From: Keith Packard Date: Sat, 5 Apr 2014 06:34:48 +0000 (-0700) Subject: altos: Rename 'core' to 'kernel' X-Git-Tag: 1.3.2.2~154 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=24167015705ae831692b95735968b04a876f935e 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 --- diff --git a/configure.ac b/configure.ac index b28b549b..27dbc6a4 100644 --- a/configure.ac +++ b/configure.ac @@ -19,7 +19,7 @@ dnl Process this file with autoconf to create configure. AC_PREREQ(2.57) AC_INIT([altos], 1.3.2.1) -AC_CONFIG_SRCDIR([src/core/ao.h]) +AC_CONFIG_SRCDIR([src/kernel/ao.h]) AM_INIT_AUTOMAKE([foreign dist-bzip2]) AM_MAINTAINER_MODE diff --git a/src/avr-demo/Makefile b/src/avr-demo/Makefile index 6d9bfea2..e21ad047 100644 --- a/src/avr-demo/Makefile +++ b/src/avr-demo/Makefile @@ -2,7 +2,7 @@ # AltOS build # # -vpath % ..:../core:../product:../drivers:../avr +vpath % ..:../kernel:../product:../drivers:../avr vpath make-altitude .. vpath make-kalman .. vpath kalman.5c ../kalman @@ -46,7 +46,7 @@ PRODUCT=AvrDemo-v0.0 MCU=atmega32u4 PRODUCT_DEF=-DAVR_DEMO IDPRODUCT=0x000a -CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../core -I.. +CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../kernel -I.. CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -Os -mcall-prologues NICKLE=nickle diff --git a/src/cc1111/Makefile.cc1111 b/src/cc1111/Makefile.cc1111 index 78b653b3..0ea30e1d 100644 --- a/src/cc1111/Makefile.cc1111 +++ b/src/cc1111/Makefile.cc1111 @@ -3,7 +3,7 @@ CC=$(SDCC) CFLAGS=--model-small --debug --opt-code-speed -DCODESIZE=$(CODESIZE) -CFLAGS += $(PRODUCT_DEF) -I. -I.. -I../core -I../cc1111 -I../drivers -I../product +CFLAGS += $(PRODUCT_DEF) -I. -I.. -I../kernel -I../cc1111 -I../drivers -I../product CODESIZE ?= 0x8000 diff --git a/src/core/altitude.h b/src/core/altitude.h deleted file mode 100644 index a278bbc6..00000000 --- a/src/core/altitude.h +++ /dev/null @@ -1,132 +0,0 @@ -/*max error 3.197865153490684 at 0.782%. Average error 0.260150920474668*/ -#define NALT 129 -#define ALT_FRAC_BITS 8 - 15835, /* 10.56 kPa 0.000% */ - 15332, /* 11.42 kPa 0.781% */ - 14868, /* 12.29 kPa 1.563% */ - 14435, /* 13.16 kPa 2.344% */ - 14030, /* 14.02 kPa 3.125% */ - 13649, /* 14.90 kPa 3.906% */ - 13290, /* 15.76 kPa 4.688% */ - 12950, /* 16.63 kPa 5.469% */ - 12627, /* 17.50 kPa 6.250% */ - 12320, /* 18.37 kPa 7.031% */ - 12027, /* 19.24 kPa 7.813% */ - 11747, /* 20.10 kPa 8.594% */ - 11479, /* 20.97 kPa 9.375% */ - 11222, /* 21.84 kPa 10.156% */ - 10975, /* 22.71 kPa 10.938% */ - 10736, /* 23.58 kPa 11.719% */ - 10504, /* 24.44 kPa 12.500% */ - 10278, /* 25.31 kPa 13.281% */ - 10059, /* 26.18 kPa 14.063% */ - 9846, /* 27.05 kPa 14.844% */ - 9638, /* 27.91 kPa 15.625% */ - 9435, /* 28.78 kPa 16.406% */ - 9237, /* 29.65 kPa 17.188% */ - 9044, /* 30.52 kPa 17.969% */ - 8855, /* 31.39 kPa 18.750% */ - 8670, /* 32.26 kPa 19.531% */ - 8490, /* 33.13 kPa 20.313% */ - 8313, /* 33.99 kPa 21.094% */ - 8140, /* 34.86 kPa 21.875% */ - 7970, /* 35.73 kPa 22.656% */ - 7803, /* 36.60 kPa 23.438% */ - 7640, /* 37.47 kPa 24.219% */ - 7480, /* 38.33 kPa 25.000% */ - 7322, /* 39.20 kPa 25.781% */ - 7168, /* 40.07 kPa 26.563% */ - 7016, /* 40.94 kPa 27.344% */ - 6867, /* 41.80 kPa 28.125% */ - 6720, /* 42.67 kPa 28.906% */ - 6575, /* 43.54 kPa 29.688% */ - 6433, /* 44.41 kPa 30.469% */ - 6294, /* 45.28 kPa 31.250% */ - 6156, /* 46.15 kPa 32.031% */ - 6020, /* 47.01 kPa 32.813% */ - 5887, /* 47.88 kPa 33.594% */ - 5755, /* 48.75 kPa 34.375% */ - 5625, /* 49.62 kPa 35.156% */ - 5497, /* 50.49 kPa 35.938% */ - 5371, /* 51.35 kPa 36.719% */ - 5247, /* 52.22 kPa 37.500% */ - 5124, /* 53.09 kPa 38.281% */ - 5003, /* 53.96 kPa 39.063% */ - 4883, /* 54.83 kPa 39.844% */ - 4765, /* 55.69 kPa 40.625% */ - 4648, /* 56.56 kPa 41.406% */ - 4533, /* 57.43 kPa 42.188% */ - 4419, /* 58.30 kPa 42.969% */ - 4307, /* 59.17 kPa 43.750% */ - 4196, /* 60.03 kPa 44.531% */ - 4086, /* 60.90 kPa 45.313% */ - 3977, /* 61.77 kPa 46.094% */ - 3870, /* 62.63 kPa 46.875% */ - 3764, /* 63.51 kPa 47.656% */ - 3659, /* 64.38 kPa 48.438% */ - 3555, /* 65.24 kPa 49.219% */ - 3453, /* 66.11 kPa 50.000% */ - 3351, /* 66.98 kPa 50.781% */ - 3250, /* 67.85 kPa 51.563% */ - 3151, /* 68.72 kPa 52.344% */ - 3052, /* 69.58 kPa 53.125% */ - 2955, /* 70.45 kPa 53.906% */ - 2858, /* 71.32 kPa 54.688% */ - 2763, /* 72.19 kPa 55.469% */ - 2668, /* 73.06 kPa 56.250% */ - 2574, /* 73.92 kPa 57.031% */ - 2482, /* 74.79 kPa 57.813% */ - 2390, /* 75.66 kPa 58.594% */ - 2298, /* 76.52 kPa 59.375% */ - 2208, /* 77.40 kPa 60.156% */ - 2119, /* 78.26 kPa 60.938% */ - 2030, /* 79.13 kPa 61.719% */ - 1942, /* 80.00 kPa 62.500% */ - 1855, /* 80.87 kPa 63.281% */ - 1769, /* 81.74 kPa 64.063% */ - 1683, /* 82.60 kPa 64.844% */ - 1598, /* 83.47 kPa 65.625% */ - 1514, /* 84.34 kPa 66.406% */ - 1430, /* 85.21 kPa 67.188% */ - 1347, /* 86.08 kPa 67.969% */ - 1265, /* 86.94 kPa 68.750% */ - 1184, /* 87.81 kPa 69.531% */ - 1103, /* 88.68 kPa 70.313% */ - 1023, /* 89.55 kPa 71.094% */ - 943, /* 90.41 kPa 71.875% */ - 864, /* 91.28 kPa 72.656% */ - 786, /* 92.15 kPa 73.438% */ - 708, /* 93.02 kPa 74.219% */ - 631, /* 93.89 kPa 75.000% */ - 554, /* 94.76 kPa 75.781% */ - 478, /* 95.63 kPa 76.563% */ - 403, /* 96.49 kPa 77.344% */ - 328, /* 97.36 kPa 78.125% */ - 254, /* 98.23 kPa 78.906% */ - 180, /* 99.10 kPa 79.688% */ - 106, /* 99.97 kPa 80.469% */ - 34, /* 100.83 kPa 81.250% */ - -39, /* 101.70 kPa 82.031% */ - -111, /* 102.57 kPa 82.813% */ - -182, /* 103.44 kPa 83.594% */ - -253, /* 104.30 kPa 84.375% */ - -323, /* 105.17 kPa 85.156% */ - -393, /* 106.04 kPa 85.938% */ - -462, /* 106.91 kPa 86.719% */ - -531, /* 107.78 kPa 87.500% */ - -600, /* 108.65 kPa 88.281% */ - -668, /* 109.51 kPa 89.063% */ - -736, /* 110.38 kPa 89.844% */ - -803, /* 111.25 kPa 90.625% */ - -870, /* 112.12 kPa 91.406% */ - -936, /* 112.99 kPa 92.188% */ - -1002, /* 113.85 kPa 92.969% */ - -1068, /* 114.72 kPa 93.750% */ - -1133, /* 115.59 kPa 94.531% */ - -1198, /* 116.46 kPa 95.313% */ - -1262, /* 117.33 kPa 96.094% */ - -1326, /* 118.19 kPa 96.875% */ - -1389, /* 119.06 kPa 97.656% */ - -1453, /* 119.93 kPa 98.438% */ - -1516, /* 120.80 kPa 99.219% */ - -1578, /* 121.67 kPa 100.000% */ diff --git a/src/core/ao.h b/src/core/ao.h deleted file mode 100644 index 29ad2603..00000000 --- a/src/core/ao.h +++ /dev/null @@ -1,1041 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_H_ -#define _AO_H_ - -#include -#include -#include -#include -#include -#include - -#define TRUE 1 -#define FALSE 0 - -/* Convert a __data pointer into an __xdata pointer */ -#ifndef DATA_TO_XDATA -#define DATA_TO_XDATA(a) (a) -#endif -#ifndef PDATA_TO_XDATA -#define PDATA_TO_XDATA(a) (a) -#endif -#ifndef CODE_TO_XDATA -#define CODE_TO_XDATA(a) (a) -#endif - -#ifndef HAS_TASK -#define HAS_TASK 1 -#endif - -#ifndef AO_PORT_TYPE -#define AO_PORT_TYPE uint8_t -#endif - -typedef AO_PORT_TYPE ao_port_t; - -#if HAS_TASK -#include -#else -#include -#endif - -/* - * ao_panic.c - */ - -#define AO_PANIC_NO_TASK 1 /* AO_NUM_TASKS is not large enough */ -#define AO_PANIC_DMA 2 /* Attempt to start DMA while active */ -#define AO_PANIC_MUTEX 3 /* Mis-using mutex API */ -#define AO_PANIC_EE 4 /* Mis-using eeprom API */ -#define AO_PANIC_LOG 5 /* Failing to read/write log data */ -#define AO_PANIC_CMD 6 /* Too many command sets registered */ -#define AO_PANIC_STDIO 7 /* Too many stdio handlers registered */ -#define AO_PANIC_REBOOT 8 /* Reboot failed */ -#define AO_PANIC_FLASH 9 /* Invalid flash part (or wrong blocksize) */ -#define AO_PANIC_USB 10 /* Trying to send USB packet while busy */ -#define AO_PANIC_BT 11 /* Communications with bluetooth device failed */ -#define AO_PANIC_STACK 12 /* Stack overflow */ -#define AO_PANIC_SPI 13 /* SPI communication failure */ -#define AO_PANIC_CRASH 14 /* Processor crashed */ -#define AO_PANIC_BUFIO 15 /* Mis-using bufio API */ -#define AO_PANIC_EXTI 16 /* Mis-using exti API */ -#define AO_PANIC_FAST_TIMER 17 /* Mis-using fast timer API */ -#define AO_PANIC_SELF_TEST_CC1120 0x40 | 1 /* Self test failure */ -#define AO_PANIC_SELF_TEST_HMC5883 0x40 | 2 /* Self test failure */ -#define AO_PANIC_SELF_TEST_MPU6000 0x40 | 3 /* Self test failure */ -#define AO_PANIC_SELF_TEST_MS5607 0x40 | 4 /* Self test failure */ - -/* Stop the operating system, beeping and blinking the reason */ -void -ao_panic(uint8_t reason); - -/* - * ao_timer.c - */ - -#ifndef AO_TICK_TYPE -#define AO_TICK_TYPE uint16_t -#define AO_TICK_SIGNED int16_t -#endif - -extern volatile __data AO_TICK_TYPE ao_tick_count; - -/* Our timer runs at 100Hz */ -#ifndef AO_HERTZ -#define AO_HERTZ 100 -#endif -#define AO_MS_TO_TICKS(ms) ((ms) / (1000 / AO_HERTZ)) -#define AO_SEC_TO_TICKS(s) ((s) * AO_HERTZ) - -/* Returns the current time in ticks */ -AO_TICK_TYPE -ao_time(void); - -/* Suspend the current task until ticks time has passed */ -void -ao_delay(uint16_t ticks); - -/* Set the ADC interval */ -void -ao_timer_set_adc_interval(uint8_t interval); - -/* Timer interrupt */ -void -ao_timer_isr(void) ao_arch_interrupt(9); - -/* Initialize the timer */ -void -ao_timer_init(void); - -/* Initialize the hardware clock. Must be called first */ -void -ao_clock_init(void); - -/* - * ao_mutex.c - */ - -#ifndef ao_mutex_get -void -ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant; - -void -ao_mutex_put(__xdata uint8_t *ao_mutex) __reentrant; -#endif - -/* - * ao_cmd.c - */ - -enum ao_cmd_status { - ao_cmd_success = 0, - ao_cmd_lex_error = 1, - ao_cmd_syntax_error = 2, -}; - -extern __pdata uint16_t ao_cmd_lex_i; -extern __pdata uint32_t ao_cmd_lex_u32; -extern __pdata char ao_cmd_lex_c; -extern __pdata enum ao_cmd_status ao_cmd_status; - -void -ao_put_string(__code char *s); - -void -ao_cmd_lex(void); - -void -ao_cmd_put8(uint8_t v); - -void -ao_cmd_put16(uint16_t v); - -uint8_t -ao_cmd_is_white(void); - -void -ao_cmd_white(void); - -int8_t -ao_cmd_hexchar(char c); - -void -ao_cmd_hexbyte(void); - -void -ao_cmd_hex(void); - -void -ao_cmd_decimal(void) __reentrant; - -/* Read a single hex nibble off stdin. */ -uint8_t -ao_getnibble(void); - -uint8_t -ao_match_word(__code char *word); - -struct ao_cmds { - void (*func)(void); - __code char *help; -}; - -void -ao_cmd_register(const __code struct ao_cmds *cmds); - -void -ao_cmd_init(void); - -#if HAS_CMD_FILTER -/* - * Provided by an external module to filter raw command lines - */ -uint8_t -ao_cmd_filter(void); -#endif - -/* - * Various drivers - */ -#if HAS_ADC -#include -#endif - -#if HAS_BEEP -#include -#endif - -#if LEDS_AVAILABLE -#include -#endif - -#if HAS_USB -#include -#endif - -#if HAS_EEPROM -#include -#endif - -#if HAS_LOG -#include -#endif - -#if HAS_FLIGHT -#include -#include -#endif - -/* - * ao_report.c - */ - -#define AO_RDF_INTERVAL_TICKS AO_SEC_TO_TICKS(5) -#define AO_RDF_LENGTH_MS 500 -#define AO_RDF_CONTINUITY_MS 32 -#define AO_RDF_CONTINUITY_PAUSE 96 -#define AO_RDF_CONTINUITY_TOTAL ((AO_RDF_CONTINUITY_PAUSE + AO_RDF_CONTINUITY_MS) * 3 + AO_RDF_CONTINUITY_PAUSE) - -/* This assumes that we're generating a 1kHz tone, which - * modulates the carrier at 2kbps, or 250kBps - */ -#define AO_MS_TO_RDF_LEN(ms) ((ms) / 4) - -#define AO_RADIO_RDF_LEN AO_MS_TO_RDF_LEN(AO_RDF_LENGTH_MS) -#define AO_RADIO_CONT_TONE_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_MS) -#define AO_RADIO_CONT_PAUSE_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_PAUSE) -#define AO_RADIO_CONT_TOTAL_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_TOTAL) - -/* returns a value 0-3 to indicate igniter continuity */ -uint8_t -ao_report_igniter(void); - -void -ao_report_init(void); - -/* - * ao_convert.c - * - * Given raw data, convert to SI units - */ - -/* pressure from the sensor to altitude in meters */ -int16_t -ao_pres_to_altitude(int16_t pres) __reentrant; - -int16_t -ao_altitude_to_pres(int16_t alt) __reentrant; - -int16_t -ao_temp_to_dC(int16_t temp) __reentrant; - -/* - * ao_convert_pa.c - * - * Convert between pressure in Pa and altitude in meters - */ - -#include - -alt_t -ao_pa_to_altitude(int32_t pa); - -int32_t -ao_altitude_to_pa(alt_t alt); - -#if HAS_DBG -#include -#endif - -#if HAS_SERIAL_0 || HAS_SERIAL_1 || HAS_SERIAL_2 || HAS_SERIAL_3 -#include -#endif - -/* - * ao_convert_volt.c - * - * Convert ADC readings to decivolts - */ - -int16_t -ao_battery_decivolt(int16_t adc); - -int16_t -ao_ignite_decivolt(int16_t adc); - -/* - * ao_spi_slave.c - */ - -uint8_t -ao_spi_slave_recv(void *buf, uint16_t len); - -void -ao_spi_slave_send(void *buf, uint16_t len); - -void -ao_spi_slave_init(void); - -/* This must be defined by the product; it will get called when chip - * select goes low, at which point it should use ao_spi_read and - * ao_spi_write to deal with the request - */ - -void -ao_spi_slave(void); - -#include -/* - * ao_gps.c - */ - -#define AO_GPS_NUM_SAT_MASK (0xf << 0) -#define AO_GPS_NUM_SAT_SHIFT (0) - -#define AO_GPS_VALID (1 << 4) -#define AO_GPS_RUNNING (1 << 5) -#define AO_GPS_DATE_VALID (1 << 6) -#define AO_GPS_COURSE_VALID (1 << 7) - -#define AO_GPS_NEW_DATA 1 -#define AO_GPS_NEW_TRACKING 2 - -extern __xdata uint8_t ao_gps_new; -extern __pdata uint16_t ao_gps_tick; -extern __xdata uint8_t ao_gps_mutex; -extern __xdata struct ao_telemetry_location ao_gps_data; -extern __xdata struct ao_telemetry_satellite ao_gps_tracking_data; - -struct ao_gps_orig { - uint8_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t minute; - uint8_t second; - uint8_t flags; - int32_t latitude; /* degrees * 10⁷ */ - int32_t longitude; /* degrees * 10⁷ */ - int16_t altitude; /* m */ - uint16_t ground_speed; /* cm/s */ - uint8_t course; /* degrees / 2 */ - uint8_t hdop; /* * 5 */ - int16_t climb_rate; /* cm/s */ - uint16_t h_error; /* m */ - uint16_t v_error; /* m */ -}; - -struct ao_gps_sat_orig { - uint8_t svid; - uint8_t c_n_1; -}; - -#define AO_MAX_GPS_TRACKING 12 - -struct ao_gps_tracking_orig { - uint8_t channels; - struct ao_gps_sat_orig sats[AO_MAX_GPS_TRACKING]; -}; - -void -ao_gps(void); - -void -ao_gps_print(__xdata struct ao_gps_orig *gps_data); - -void -ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data); - -void -ao_gps_show(void) __reentrant; - -void -ao_gps_init(void); - -/* - * ao_gps_report.c - */ - -void -ao_gps_report(void); - -void -ao_gps_report_init(void); - -/* - * ao_gps_report_mega.c - */ - -void -ao_gps_report_mega(void); - -void -ao_gps_report_mega_init(void); - -/* - * ao_telemetry_orig.c - */ - -#if LEGACY_MONITOR -struct ao_adc_orig { - uint16_t tick; /* tick when the sample was read */ - int16_t accel; /* accelerometer */ - int16_t pres; /* pressure sensor */ - int16_t temp; /* temperature sensor */ - int16_t v_batt; /* battery voltage */ - int16_t sense_d; /* drogue continuity sense */ - int16_t sense_m; /* main continuity sense */ -}; - -struct ao_telemetry_orig { - uint16_t serial; - uint16_t flight; - uint8_t flight_state; - int16_t accel; - int16_t ground_accel; - union { - struct { - int16_t speed; - int16_t unused; - } k; - int32_t flight_vel; - } u; - int16_t height; - int16_t ground_pres; - int16_t accel_plus_g; - int16_t accel_minus_g; - struct ao_adc_orig adc; - struct ao_gps_orig gps; - char callsign[AO_MAX_CALLSIGN]; - struct ao_gps_tracking_orig gps_tracking; -}; - -struct ao_telemetry_tiny { - uint16_t serial; - uint16_t flight; - uint8_t flight_state; - int16_t height; /* AGL in meters */ - int16_t speed; /* in m/s * 16 */ - int16_t accel; /* in m/s² * 16 */ - int16_t ground_pres; /* sensor units */ - struct ao_adc adc; /* raw ADC readings */ - char callsign[AO_MAX_CALLSIGN]; -}; - -struct ao_telemetry_orig_recv { - struct ao_telemetry_orig telemetry_orig; - int8_t rssi; - uint8_t status; -}; - -struct ao_telemetry_tiny_recv { - struct ao_telemetry_tiny telemetry_tiny; - int8_t rssi; - uint8_t status; -}; - -#endif /* LEGACY_MONITOR */ - -/* Unfortunately, we've exposed the CC1111 rssi units as the 'usual' method - * for reporting RSSI. So, now we use these values everywhere - */ -#define AO_RSSI_FROM_RADIO(radio) ((int16_t) ((int8_t) (radio) >> 1) - 74) -#define AO_RADIO_FROM_RSSI(rssi) (((int8_t) (rssi) + 74) << 1) - -/* - * ao_radio_recv tacks on rssi and status bytes - */ - -struct ao_telemetry_raw_recv { - uint8_t packet[AO_MAX_TELEMETRY + 2]; -}; - -/* Set delay between telemetry reports (0 to disable) */ - -#ifdef AO_SEND_ALL_BARO -#define AO_TELEMETRY_INTERVAL_PAD AO_MS_TO_TICKS(100) -#define AO_TELEMETRY_INTERVAL_FLIGHT AO_MS_TO_TICKS(100) -#define AO_TELEMETRY_INTERVAL_RECOVER AO_MS_TO_TICKS(100) -#else -#define AO_TELEMETRY_INTERVAL_PAD AO_MS_TO_TICKS(1000) -#define AO_TELEMETRY_INTERVAL_FLIGHT AO_MS_TO_TICKS(100) -#define AO_TELEMETRY_INTERVAL_RECOVER AO_MS_TO_TICKS(1000) -#endif - -void -ao_telemetry_set_interval(uint16_t interval); - -void -ao_rdf_set(uint8_t rdf); - -void -ao_telemetry_init(void); - -void -ao_telemetry_orig_init(void); - -void -ao_telemetry_tiny_init(void); - -/* - * ao_radio.c - */ - -extern __xdata uint8_t ao_radio_dma; - -extern __xdata int8_t ao_radio_rssi; - -#ifdef PKT_APPEND_STATUS_1_CRC_OK -#define AO_RADIO_STATUS_CRC_OK PKT_APPEND_STATUS_1_CRC_OK -#else -#include -#define AO_RADIO_STATUS_CRC_OK AO_FEC_DECODE_CRC_OK -#endif - -#ifndef HAS_RADIO_RECV -#define HAS_RADIO_RECV HAS_RADIO -#endif -#ifndef HAS_RADIO_XMIT -#define HAS_RADIO_XMIT HAS_RADIO -#endif - -void -ao_radio_general_isr(void) ao_arch_interrupt(16); - -#if HAS_RADIO_XMIT -void -ao_radio_send(const __xdata void *d, uint8_t size) __reentrant; -#endif - -#if HAS_RADIO_RECV -uint8_t -ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout) __reentrant; - -void -ao_radio_recv_abort(void); -#endif - -void -ao_radio_test(uint8_t on); - -typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len); - -void -ao_radio_send_aprs(ao_radio_fill_func fill); - -/* - * ao_radio_pa - */ - -#if HAS_RADIO_AMP -void -ao_radio_pa_on(void); - -void -ao_radio_pa_off(void); - -void -ao_radio_pa_init(void); -#else -#define ao_radio_pa_on() -#define ao_radio_pa_off() -#define ao_radio_pa_init() -#endif - -/* - * Compute the packet length as follows: - * - * 2000 bps (for a 1kHz tone) - * so, for 'ms' milliseconds, we need - * 2 * ms bits, or ms / 4 bytes - */ - -void -ao_radio_rdf(void); - -void -ao_radio_continuity(uint8_t c); - -void -ao_radio_rdf_abort(void); - -void -ao_radio_init(void); - -/* - * ao_monitor.c - */ - -#if HAS_MONITOR - -extern const char const * const ao_state_names[]; - -#define AO_MONITOR_RING 8 - -union ao_monitor { - struct ao_telemetry_raw_recv raw; - struct ao_telemetry_all_recv all; -#if LEGACY_MONITOR - struct ao_telemetry_orig_recv orig; - struct ao_telemetry_tiny_recv tiny; -#endif -}; - -extern __xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; - -#define ao_monitor_ring_next(n) (((n) + 1) & (AO_MONITOR_RING - 1)) - -extern __data uint8_t ao_monitoring; -extern __data uint8_t ao_monitor_head; - -void -ao_monitor(void); - -#define AO_MONITORING_OFF 0 -#define AO_MONITORING_ORIG 1 - -void -ao_monitor_set(uint8_t monitoring); - -void -ao_monitor_disable(void); - -void -ao_monitor_enable(void); - -void -ao_monitor_init(void) __reentrant; - -#endif - -/* - * ao_stdio.c - */ - -#define AO_READ_AGAIN (-1) - -struct ao_stdio { - int (*_pollchar)(void); /* Called with interrupts blocked */ - void (*putchar)(char c) __reentrant; - void (*flush)(void); - uint8_t echo; -}; - -extern __xdata struct ao_stdio ao_stdios[]; -extern __pdata int8_t ao_cur_stdio; -extern __pdata int8_t ao_num_stdios; - -void -flush(void); - -extern __xdata uint8_t ao_stdin_ready; - -uint8_t -ao_echo(void); - -int8_t -ao_add_stdio(int (*pollchar)(void), - void (*putchar)(char) __reentrant, - void (*flush)(void)) __reentrant; - -/* - * ao_ignite.c - */ - -enum ao_igniter { - ao_igniter_drogue = 0, - ao_igniter_main = 1 -}; - -void -ao_ignite(enum ao_igniter igniter); - -enum ao_igniter_status { - ao_igniter_unknown, /* unknown status (ambiguous voltage) */ - ao_igniter_ready, /* continuity detected */ - ao_igniter_active, /* igniter firing */ - ao_igniter_open, /* open circuit detected */ -}; - -struct ao_ignition { - uint8_t request; - uint8_t fired; - uint8_t firing; -}; - -extern __code char * __code ao_igniter_status_names[]; - -extern __xdata struct ao_ignition ao_ignition[2]; - -enum ao_igniter_status -ao_igniter_status(enum ao_igniter igniter); - -extern __pdata uint8_t ao_igniter_present; - -void -ao_ignite_set_pins(void); - -void -ao_igniter_init(void); - -/* - * ao_config.c - */ - -#if AO_PYRO_NUM -#include -#endif - -#if HAS_FORCE_FREQ -/* - * Set this to force the frequency to 434.550MHz - */ -extern __xdata uint8_t ao_force_freq; -#endif - -#define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 15 - -#define AO_AES_LEN 16 - -extern __xdata uint8_t ao_config_aes_seq; - -struct ao_config { - uint8_t major; - uint8_t minor; - uint16_t main_deploy; - int16_t accel_plus_g; /* changed for minor version 2 */ - uint8_t _legacy_radio_channel; - char callsign[AO_MAX_CALLSIGN + 1]; - uint8_t apogee_delay; /* minor version 1 */ - int16_t accel_minus_g; /* minor version 2 */ - uint32_t radio_cal; /* minor version 3 */ - uint32_t flight_log_max; /* minor version 4 */ - uint8_t ignite_mode; /* minor version 5 */ - uint8_t pad_orientation; /* minor version 6 */ - uint32_t radio_setting; /* minor version 7 */ - uint8_t radio_enable; /* minor version 8 */ - uint8_t aes_key[AO_AES_LEN]; /* minor version 9 */ - uint32_t frequency; /* minor version 10 */ - uint16_t apogee_lockout; /* minor version 11 */ -#if AO_PYRO_NUM - struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ -#endif - uint16_t aprs_interval; /* minor version 13 */ -#if HAS_RADIO_POWER - uint8_t radio_power; /* minor version 14 */ -#endif -#if HAS_RADIO_AMP - uint8_t radio_amp; /* minor version 14 */ -#endif -#if HAS_GYRO - int16_t accel_zero_along; /* minor version 15 */ - int16_t accel_zero_across; /* minor version 15 */ - int16_t accel_zero_through; /* minor version 15 */ -#endif -}; - -#define AO_IGNITE_MODE_DUAL 0 -#define AO_IGNITE_MODE_APOGEE 1 -#define AO_IGNITE_MODE_MAIN 2 - -#define AO_RADIO_ENABLE_CORE 1 -#define AO_RADIO_DISABLE_TELEMETRY 2 -#define AO_RADIO_DISABLE_RDF 4 - -#define AO_PAD_ORIENTATION_ANTENNA_UP 0 -#define AO_PAD_ORIENTATION_ANTENNA_DOWN 1 - -extern __xdata struct ao_config ao_config; - -#define AO_CONFIG_MAX_SIZE 128 - -void -_ao_config_edit_start(void); - -void -_ao_config_edit_finish(void); - -void -ao_config_get(void); - -void -ao_config_put(void); - -void -ao_config_set_radio(void); - -void -ao_config_init(void); - -/* - * ao_rssi.c - */ - -void -ao_rssi_set(int rssi_value); - -void -ao_rssi_init(uint8_t rssi_led); - -/* - * ao_product.c - * - * values which need to be defined for - * each instance of a product - */ - -extern const char ao_version[]; -extern const char ao_manufacturer[]; -extern const char ao_product[]; - -/* - * Fifos - */ - -#define AO_FIFO_SIZE 32 - -struct ao_fifo { - uint8_t insert; - uint8_t remove; - char fifo[AO_FIFO_SIZE]; -}; - -#define ao_fifo_insert(f,c) do { \ - (f).fifo[(f).insert] = (c); \ - (f).insert = ((f).insert + 1) & (AO_FIFO_SIZE-1); \ -} while(0) - -#define ao_fifo_remove(f,c) do {\ - c = (f).fifo[(f).remove]; \ - (f).remove = ((f).remove + 1) & (AO_FIFO_SIZE-1); \ -} while(0) - -#define ao_fifo_full(f) ((((f).insert + 1) & (AO_FIFO_SIZE-1)) == (f).remove) -#define ao_fifo_empty(f) ((f).insert == (f).remove) - -#if PACKET_HAS_MASTER || PACKET_HAS_SLAVE -#include -#endif - -#if HAS_BTM -#include -#endif - -#if HAS_COMPANION -#include -#endif - -#if HAS_LCD -#include -#endif - -#if HAS_AES -#include -#endif - -/* ao_launch.c */ - -struct ao_launch_command { - uint16_t tick; - uint16_t serial; - uint8_t cmd; - uint8_t channel; - uint16_t unused; -}; - -#define AO_LAUNCH_QUERY 1 - -struct ao_launch_query { - uint16_t tick; - uint16_t serial; - uint8_t channel; - uint8_t valid; - uint8_t arm_status; - uint8_t igniter_status; -}; - -#define AO_LAUNCH_ARM 2 -#define AO_LAUNCH_FIRE 3 - -void -ao_launch_init(void); - -/* - * ao_log_single.c - */ - -#define AO_LOG_TELESCIENCE_START ((uint8_t) 's') -#define AO_LOG_TELESCIENCE_DATA ((uint8_t) 'd') - -#define AO_LOG_TELESCIENCE_NUM_ADC 12 - -struct ao_log_telescience { - uint8_t type; - uint8_t csum; - uint16_t tick; - uint16_t tm_tick; - uint8_t tm_state; - uint8_t unused; - uint16_t adc[AO_LOG_TELESCIENCE_NUM_ADC]; -}; - -#define AO_LOG_SINGLE_SIZE 32 - -union ao_log_single { - struct ao_log_telescience telescience; - union ao_telemetry_all telemetry; - uint8_t bytes[AO_LOG_SINGLE_SIZE]; -}; - -extern __xdata union ao_log_single ao_log_single_write_data; -extern __xdata union ao_log_single ao_log_single_read_data; - -void -ao_log_single_extra_query(void); - -void -ao_log_single_list(void); - -void -ao_log_single_main(void); - -uint8_t -ao_log_single_write(void); - -uint8_t -ao_log_single_read(uint32_t pos); - -void -ao_log_single_start(void); - -void -ao_log_single_stop(void); - -void -ao_log_single_restart(void); - -void -ao_log_single_set(void); - -void -ao_log_single_delete(void); - -void -ao_log_single_init(void); - -void -ao_log_single(void); - -/* - * ao_pyro_slave.c - */ - -#define AO_TELEPYRO_NUM_ADC 9 - -#ifndef ao_xmemcpy -#define ao_xmemcpy(d,s,c) memcpy(d,s,c) -#define ao_xmemset(d,v,c) memset(d,v,c) -#define ao_xmemcmp(d,s,c) memcmp(d,s,c) -#endif - -/* - * ao_terraui.c - */ - -void -ao_terraui_init(void); - -/* - * ao_battery.c - */ - -#ifdef BATTERY_PIN -void -ao_battery_isr(void) ao_arch_interrupt(1); - -uint16_t -ao_battery_get(void); - -void -ao_battery_init(void); -#endif /* BATTERY_PIN */ - -/* - * ao_sqrt.c - */ - -uint32_t -ao_sqrt(uint32_t op); - -/* - * ao_freq.c - */ - -int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant; - -/* - * ao_ms5607.c - */ - -void ao_ms5607_init(void); - -#include - -#endif /* _AO_H_ */ diff --git a/src/core/ao_adc.h b/src/core/ao_adc.h deleted file mode 100644 index 373db1c4..00000000 --- a/src/core/ao_adc.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_ADC_H_ -#define _AO_ADC_H_ - -#include - -/* Trigger a conversion sequence (called from the timer interrupt) */ -void -ao_adc_poll(void); - -/* Suspend the current task until another A/D sample is converted */ -void -ao_adc_sleep(void); - -/* Initialize the A/D converter */ -void -ao_adc_init(void); - -#endif /* _AO_ADC_H_ */ diff --git a/src/core/ao_aes.h b/src/core/ao_aes.h deleted file mode 100644 index c47bc2db..00000000 --- a/src/core/ao_aes.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_AES_H_ -#define _AO_AES_H_ - -/* ao_aes.c */ - -extern __xdata uint8_t ao_aes_mutex; - -/* AES keys and blocks are 128 bits */ - -enum ao_aes_mode { - ao_aes_mode_cbc_mac -}; - -#if HAS_AES -#ifdef SDCC -void -ao_aes_isr(void) __interrupt 4; -#endif -#endif - -void -ao_aes_set_mode(enum ao_aes_mode mode); - -void -ao_aes_set_key(__xdata uint8_t *in); - -void -ao_aes_zero_iv(void); - -void -ao_aes_run(__xdata uint8_t *in, - __xdata uint8_t *out); - -void -ao_aes_init(void); - -#endif /* _AO_AES_H_ */ diff --git a/src/core/ao_balloon.c b/src/core/ao_balloon.c deleted file mode 100644 index 904a9c08..00000000 --- a/src/core/ao_balloon.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_FLIGHT_TEST -#include "ao.h" -#endif - -#ifndef HAS_ACCEL -#error Please define HAS_ACCEL -#endif - -#ifndef HAS_GPS -#error Please define HAS_GPS -#endif - -#ifndef HAS_USB -#error Please define HAS_USB -#endif - -#if HAS_SENSOR_ERRORS -/* Any sensor can set this to mark the flight computer as 'broken' */ -__xdata uint8_t ao_sensor_errors; -#endif - -__pdata uint16_t ao_motor_number; /* number of motors burned so far */ - -/* Main flight thread. */ - -__pdata enum ao_flight_state ao_flight_state; /* current flight state */ - -__pdata uint8_t ao_flight_force_idle; - -void -ao_flight(void) -{ - ao_sample_init(); - ao_flight_state = ao_flight_startup; - for (;;) { - - /* - * Process ADC samples, just looping - * until the sensors are calibrated. - */ - if (!ao_sample()) - continue; - - switch (ao_flight_state) { - case ao_flight_startup: - - /* Check to see what mode we should go to. - * - Invalid mode if accel cal appears to be out - * - pad mode if we're upright, - * - idle mode otherwise - */ - if (!ao_flight_force_idle) - { - /* Set pad mode - we can fly! */ - ao_flight_state = ao_flight_pad; -#if HAS_USB - /* Disable the USB controller in flight mode - * to save power - */ - if (!ao_usb_running) - ao_usb_disable(); -#endif - - /* Disable packet mode in pad state */ - ao_packet_slave_stop(); - - /* Turn on telemetry system */ - ao_rdf_set(1); - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_BALLOON); - - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); - } else { - /* Set idle mode */ - ao_flight_state = ao_flight_idle; - - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); - } - /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - - break; - case ao_flight_pad: - - /* pad to coast: - * - * barometer: > 20m vertical motion - */ - if (ao_height > AO_M_TO_HEIGHT(20)) - { - ao_flight_state = ao_flight_drogue; - - /* start logging data */ - ao_log_start(); - -#if HAS_GPS - /* Record current GPS position by waking up GPS log tasks */ - ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; - ao_wakeup(&ao_gps_new); -#endif - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; - default: - break; - } - } -} - -static __xdata struct ao_task flight_task; - -void -ao_flight_init(void) -{ - ao_flight_state = ao_flight_startup; - ao_add_task(&flight_task, ao_flight, "flight"); -} diff --git a/src/core/ao_beep.h b/src/core/ao_beep.h deleted file mode 100644 index 55f61171..00000000 --- a/src/core/ao_beep.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_BEEP_H_ -#define _AO_BEEP_H_ - -/* - * ao_beep.c - */ - -/* - * Various pre-defined beep frequencies - * - * frequency = 1/2 (24e6/32) / beep - */ - -#define AO_BEEP_LOW 150 /* 2500Hz */ -#define AO_BEEP_MID 94 /* 3989Hz */ -#define AO_BEEP_HIGH 75 /* 5000Hz */ -#define AO_BEEP_OFF 0 /* off */ - -#define AO_BEEP_g 240 /* 1562.5Hz */ -#define AO_BEEP_gs 227 /* 1652Hz (1655Hz) */ -#define AO_BEEP_aa 214 /* 1752Hz (1754Hz) */ -#define AO_BEEP_bbf 202 /* 1856Hz (1858Hz) */ -#define AO_BEEP_bb 190 /* 1974Hz (1969Hz) */ -#define AO_BEEP_cc 180 /* 2083Hz (2086Hz) */ -#define AO_BEEP_ccs 170 /* 2205Hz (2210Hz) */ -#define AO_BEEP_dd 160 /* 2344Hz (2341Hz) */ -#define AO_BEEP_eef 151 /* 2483Hz (2480Hz) */ -#define AO_BEEP_ee 143 /* 2622Hz (2628Hz) */ -#define AO_BEEP_ff 135 /* 2778Hz (2784Hz) */ -#define AO_BEEP_ffs 127 /* 2953Hz (2950Hz) */ -#define AO_BEEP_gg 120 /* 3125Hz */ -#define AO_BEEP_ggs 113 /* 3319Hz (3311Hz) */ -#define AO_BEEP_aaa 107 /* 3504Hz (3508Hz) */ -#define AO_BEEP_bbbf 101 /* 3713Hz (3716Hz) */ -#define AO_BEEP_bbb 95 /* 3947Hz (3937Hz) */ -#define AO_BEEP_ccc 90 /* 4167Hz (4171Hz) */ -#define AO_BEEP_cccs 85 /* 4412Hz (4419Hz) */ -#define AO_BEEP_ddd 80 /* 4688Hz (4682Hz) */ -#define AO_BEEP_eeef 76 /* 4934Hz (4961Hz) */ -#define AO_BEEP_eee 71 /* 5282Hz (5256Hz) */ -#define AO_BEEP_fff 67 /* 5597Hz (5568Hz) */ -#define AO_BEEP_fffs 64 /* 5859Hz (5899Hz) */ -#define AO_BEEP_ggg 60 /* 6250Hz */ - -/* Set the beeper to the specified tone */ -void -ao_beep(uint8_t beep); - -/* Turn on the beeper for the specified time */ -void -ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant; - -/* Initialize the beeper */ -void -ao_beep_init(void); - -#endif /* _AO_BEEP_H_ */ diff --git a/src/core/ao_btm.h b/src/core/ao_btm.h deleted file mode 100644 index 484e5d7f..00000000 --- a/src/core/ao_btm.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_BTM_H_ -#define _AO_BTM_H_ - -/* ao_btm.c */ - -/* If bt_link is on P2, this interrupt is shared by USB, so the USB - * code calls this function. Otherwise, it's a regular ISR. - */ - -void -ao_btm_isr(void) -#if BT_LINK_ON_P1 - __interrupt 15 -#endif - ; -void -ao_btm_init(void); - -#endif /* _AO_BTM_H_ */ diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c deleted file mode 100644 index 4ebaa607..00000000 --- a/src/core/ao_cmd.c +++ /dev/null @@ -1,420 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include "ao_task.h" - -__pdata uint16_t ao_cmd_lex_i; -__pdata uint32_t ao_cmd_lex_u32; -__pdata char ao_cmd_lex_c; -__pdata enum ao_cmd_status ao_cmd_status; - -#define CMD_LEN 48 - -static __xdata char cmd_line[CMD_LEN]; -static __pdata uint8_t cmd_len; -static __pdata uint8_t cmd_i; - -void -ao_put_string(__code char *s) -{ - char c; - while ((c = *s++)) - putchar(c); -} - -static void -backspace(void) -{ - ao_put_string ("\010 \010"); -} - -static void -readline(void) -{ - char c; - if (ao_echo()) - ao_put_string("> "); - cmd_len = 0; - for (;;) { - flush(); - c = getchar(); - /* backspace/delete */ - if (c == '\010' || c == '\177') { - if (cmd_len != 0) { - if (ao_echo()) - backspace(); - --cmd_len; - } - continue; - } - - /* ^U */ - if (c == '\025') { - while (cmd_len != 0) { - if (ao_echo()) - backspace(); - --cmd_len; - } - continue; - } - - /* map CR to NL */ - if (c == '\r') - c = '\n'; - - if (c == '\n') { - if (ao_echo()) - putchar('\n'); - break; - } - - if (cmd_len >= CMD_LEN - 2) - continue; - cmd_line[cmd_len++] = c; - if (ao_echo()) - putchar(c); - } - cmd_line[cmd_len++] = '\n'; - cmd_line[cmd_len++] = '\0'; - cmd_i = 0; -} - -void -ao_cmd_lex(void) -{ - ao_cmd_lex_c = '\n'; - if (cmd_i < cmd_len) - ao_cmd_lex_c = cmd_line[cmd_i++]; -} - -static void -putnibble(uint8_t v) -{ - if (v < 10) - putchar(v + '0'); - else - putchar(v + ('a' - 10)); -} - -uint8_t -ao_getnibble(void) -{ - char c; - - c = getchar(); - if ('0' <= c && c <= '9') - return c - '0'; - if ('a' <= c && c <= 'f') - return c - ('a' - 10); - if ('A' <= c && c <= 'F') - return c - ('A' - 10); - ao_cmd_status = ao_cmd_lex_error; - return 0; -} - -void -ao_cmd_put16(uint16_t v) -{ - ao_cmd_put8(v >> 8); - ao_cmd_put8(v); -} - -void -ao_cmd_put8(uint8_t v) -{ - putnibble((v >> 4) & 0xf); - putnibble(v & 0xf); -} - -uint8_t -ao_cmd_is_white(void) -{ - return ao_cmd_lex_c == ' ' || ao_cmd_lex_c == '\t'; -} - -void -ao_cmd_white(void) -{ - while (ao_cmd_is_white()) - ao_cmd_lex(); -} - -int8_t -ao_cmd_hexchar(char c) -{ - if ('0' <= c && c <= '9') - return (c - '0'); - if ('a' <= c && c <= 'f') - return (c - 'a' + 10); - if ('A' <= c && c <= 'F') - return (c - 'A' + 10); - return -1; -} - -void -ao_cmd_hexbyte(void) -{ - uint8_t i; - int8_t n; - - ao_cmd_lex_i = 0; - ao_cmd_white(); - for (i = 0; i < 2; i++) { - n = ao_cmd_hexchar(ao_cmd_lex_c); - if (n < 0) { - ao_cmd_status = ao_cmd_syntax_error; - break; - } - ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n; - ao_cmd_lex(); - } -} - -void -ao_cmd_hex(void) -{ - __pdata uint8_t r = ao_cmd_lex_error; - int8_t n; - - ao_cmd_lex_i = 0; - ao_cmd_white(); - for(;;) { - n = ao_cmd_hexchar(ao_cmd_lex_c); - if (n < 0) - break; - ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n; - r = ao_cmd_success; - ao_cmd_lex(); - } - if (r != ao_cmd_success) - ao_cmd_status = r; -} - -void -ao_cmd_decimal(void) __reentrant -{ - uint8_t r = ao_cmd_lex_error; - - ao_cmd_lex_u32 = 0; - ao_cmd_white(); - for(;;) { - if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') - ao_cmd_lex_u32 = (ao_cmd_lex_u32 * 10) + (ao_cmd_lex_c - '0'); - else - break; - r = ao_cmd_success; - ao_cmd_lex(); - } - if (r != ao_cmd_success) - ao_cmd_status = r; - ao_cmd_lex_i = (uint16_t) ao_cmd_lex_u32; -} - -uint8_t -ao_match_word(__code char *word) -{ - while (*word) { - if (ao_cmd_lex_c != *word) { - ao_cmd_status = ao_cmd_syntax_error; - return 0; - } - word++; - ao_cmd_lex(); - } - return 1; -} - -static void -echo(void) -{ - ao_cmd_hex(); - if (ao_cmd_status == ao_cmd_success) - ao_stdios[ao_cur_stdio].echo = ao_cmd_lex_i != 0; -} - -static void -ao_reboot(void) -{ - ao_cmd_white(); - if (!ao_match_word("eboot")) - return; - /* Delay waiting for the packet master to be turned off - * so that we don't end up back in idle mode because we - * received a packet after boot. - */ - flush(); - ao_delay(AO_SEC_TO_TICKS(1)); - ao_arch_reboot(); - ao_panic(AO_PANIC_REBOOT); -} - -#ifndef HAS_VERSION -#define HAS_VERSION 1 -#endif - -#if HAS_VERSION -static void -version(void) -{ - printf("manufacturer %s\n" - "product %s\n" - "serial-number %u\n" -#if HAS_FLIGHT - "current-flight %u\n" -#endif -#if HAS_LOG - "log-format %u\n" -#endif - , ao_manufacturer - , ao_product - , ao_serial_number -#if HAS_FLIGHT - , ao_flight_number -#endif -#if HAS_LOG - , ao_log_format -#endif - ); - printf("software-version %s\n", ao_version); -} -#endif - -#ifndef NUM_CMDS -#define NUM_CMDS 11 -#endif - -static __code struct ao_cmds *__xdata (ao_cmds[NUM_CMDS]); -static __pdata uint8_t ao_ncmds; - -static void -help(void) -{ - __pdata uint8_t cmds; - __pdata uint8_t cmd; - __code struct ao_cmds * __pdata cs; - __code const char *h; - uint8_t e; - - for (cmds = 0; cmds < ao_ncmds; cmds++) { - cs = ao_cmds[cmds]; - for (cmd = 0; cs[cmd].func; cmd++) { - h = cs[cmd].help; - ao_put_string(h); - e = strlen(h); - h += e + 1; - e = 45 - e; - while (e--) - putchar(' '); - ao_put_string(h); - putchar('\n'); - } - } -} - -static void -report(void) -{ - switch(ao_cmd_status) { - case ao_cmd_lex_error: - case ao_cmd_syntax_error: - puts("Syntax error"); - ao_cmd_status = 0; - default: - break; - } -} - -void -ao_cmd_register(__code struct ao_cmds *cmds) -{ - if (ao_ncmds >= NUM_CMDS) - ao_panic(AO_PANIC_CMD); - ao_cmds[ao_ncmds++] = cmds; -} - -void -ao_cmd(void) -{ - __pdata char c; - uint8_t cmd, cmds; - __code struct ao_cmds * __xdata cs; - void (*__xdata func)(void); - - for (;;) { - readline(); - ao_cmd_lex(); - ao_cmd_white(); - c = ao_cmd_lex_c; - ao_cmd_lex(); - if (c == '\r' || c == '\n') - continue; - func = (void (*)(void)) NULL; - for (cmds = 0; cmds < ao_ncmds; cmds++) { - cs = ao_cmds[cmds]; - for (cmd = 0; cs[cmd].func; cmd++) - if (cs[cmd].help[0] == c) { - func = cs[cmd].func; - break; - } - if (func) - break; - } - if (func) - (*func)(); - else - ao_cmd_status = ao_cmd_syntax_error; - report(); - } -} - -#if HAS_BOOT_LOADER - -#include - -static void -ao_loader(void) -{ - flush(); - ao_boot_loader(); -} -#endif - -__xdata struct ao_task ao_cmd_task; - -__code struct ao_cmds ao_base_cmds[] = { - { help, "?\0Help" }, -#if HAS_TASK_INFO - { ao_task_info, "T\0Tasks" }, -#endif - { echo, "E <0 off, 1 on>\0Echo" }, - { ao_reboot, "r eboot\0Reboot" }, -#if HAS_VERSION - { version, "v\0Version" }, -#endif -#if HAS_BOOT_LOADER - { ao_loader, "X\0Switch to boot loader" }, -#endif - { 0, NULL }, -}; - -void -ao_cmd_init(void) -{ - ao_cmd_register(&ao_base_cmds[0]); - ao_add_task(&ao_cmd_task, ao_cmd, "cmd"); -} diff --git a/src/core/ao_companion.h b/src/core/ao_companion.h deleted file mode 100644 index 035325a3..00000000 --- a/src/core/ao_companion.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_COMPANION_H_ -#define _AO_COMPANION_H_ - -/* ao_companion.c */ - -#define AO_COMPANION_SETUP 1 -#define AO_COMPANION_FETCH 2 -#define AO_COMPANION_NOTIFY 3 - -struct ao_companion_command { - uint8_t command; - uint8_t flight_state; - uint16_t tick; - uint16_t serial; - uint16_t flight; - int16_t accel; - int16_t speed; - int16_t height; - int16_t motor_number; -}; - -struct ao_companion_setup { - uint16_t board_id; - uint16_t board_id_inverse; - uint8_t update_period; - uint8_t channels; -}; - -extern __pdata uint8_t ao_companion_running; -extern __xdata uint8_t ao_companion_mutex; -extern __xdata struct ao_companion_command ao_companion_command; -extern __xdata struct ao_companion_setup ao_companion_setup; -extern __xdata uint16_t ao_companion_data[AO_COMPANION_MAX_CHANNELS]; - -void -ao_companion_init(void); - -#endif /* _AO_COMPANION_H_ */ diff --git a/src/core/ao_config.c b/src/core/ao_config.c deleted file mode 100644 index 4482f673..00000000 --- a/src/core/ao_config.c +++ /dev/null @@ -1,805 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include "ao_log.h" -#include -#if HAS_FLIGHT -#include -#include -#endif - -__xdata struct ao_config ao_config; -__pdata uint8_t ao_config_loaded; -__pdata uint8_t ao_config_dirty; -__xdata uint8_t ao_config_mutex; - -#ifndef AO_CONFIG_DEFAULT_APRS_INTERVAL -#define AO_CONFIG_DEFAULT_APRS_INTERVAL 0 -#endif -#define AO_CONFIG_DEFAULT_MAIN_DEPLOY 250 -#define AO_CONFIG_DEFAULT_RADIO_CHANNEL 0 -#define AO_CONFIG_DEFAULT_CALLSIGN "N0CALL" -#define AO_CONFIG_DEFAULT_ACCEL_ZERO_G 16000 -#define AO_CONFIG_DEFAULT_APOGEE_DELAY 0 -#define AO_CONFIG_DEFAULT_IGNITE_MODE AO_IGNITE_MODE_DUAL -#define AO_CONFIG_DEFAULT_PAD_ORIENTATION AO_PAD_ORIENTATION_ANTENNA_UP -#if HAS_EEPROM -#ifndef USE_INTERNAL_FLASH -#error Please define USE_INTERNAL_FLASH -#endif -#endif -#ifndef AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX -#if USE_INTERNAL_FLASH -#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ao_storage_config -#else -#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 192 * (uint32_t) 1024) -#endif -#endif -#ifndef AO_CONFIG_DEFAULT_RADIO_POWER -#define AO_CONFIG_DEFAULT_RADIO_POWER 0x60 -#endif -#define AO_CONFIG_DEFAULT_RADIO_AMP 0 - -#if HAS_EEPROM -static void -_ao_config_put(void) -{ - ao_config_setup(); - ao_config_erase(); - ao_config_write(0, &ao_config, sizeof (ao_config)); -#if HAS_FLIGHT - ao_log_write_erase(0); -#endif - ao_config_flush(); -} - -void -ao_config_put(void) -{ - ao_mutex_get(&ao_config_mutex); - _ao_config_put(); - ao_mutex_put(&ao_config_mutex); -} -#endif - -#if HAS_RADIO -void -ao_config_set_radio(void) -{ - ao_config.radio_setting = ao_freq_to_set(ao_config.frequency, ao_config.radio_cal); -} -#endif /* HAS_RADIO */ - -static void -_ao_config_get(void) -{ - uint8_t minor; - - if (ao_config_loaded) - return; -#if HAS_EEPROM - /* Yes, I know ao_storage_read calls ao_storage_setup, - * but ao_storage_setup *also* sets ao_storage_config, which we - * need before calling ao_storage_read here - */ - ao_config_setup(); - ao_config_read(0, &ao_config, sizeof (ao_config)); -#endif - if (ao_config.major != AO_CONFIG_MAJOR) { - ao_config.major = AO_CONFIG_MAJOR; - ao_config.minor = 0; - - /* Version 0 stuff */ - ao_config.main_deploy = AO_CONFIG_DEFAULT_MAIN_DEPLOY; - ao_xmemset(&ao_config.callsign, '\0', sizeof (ao_config.callsign)); - ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN), - sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); - ao_config._legacy_radio_channel = 0; - } - minor = ao_config.minor; - if (minor != AO_CONFIG_MINOR) { - /* Fixups for minor version 1 */ - if (minor < 1) - ao_config.apogee_delay = AO_CONFIG_DEFAULT_APOGEE_DELAY; - /* Fixups for minor version 2 */ - if (minor < 2) { - ao_config.accel_plus_g = 0; - ao_config.accel_minus_g = 0; - } - /* Fixups for minor version 3 */ -#if HAS_RADIO - if (minor < 3) - ao_config.radio_cal = ao_radio_cal; -#endif - /* Fixups for minor version 4 */ -#if HAS_FLIGHT - if (minor < 4) - ao_config.flight_log_max = AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX; -#endif - /* Fixupes for minor version 5 */ - if (minor < 5) - ao_config.ignite_mode = AO_CONFIG_DEFAULT_IGNITE_MODE; - if (minor < 6) - ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION; - if (minor < 8) - ao_config.radio_enable = AO_RADIO_ENABLE_CORE; - if (minor < 9) - ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN); - if (minor < 10) - ao_config.frequency = 434550 + ao_config._legacy_radio_channel * 100; - if (minor < 11) - ao_config.apogee_lockout = 0; -#if AO_PYRO_NUM - if (minor < 12) - memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro)); -#endif - if (minor < 13) - ao_config.aprs_interval = AO_CONFIG_DEFAULT_APRS_INTERVAL; -#if HAS_RADIO_POWER - if (minor < 14) - ao_config.radio_power = AO_CONFIG_DEFAULT_RADIO_POWER; - #endif -#if HAS_RADIO_AMP - if (minor < 14) - ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP; -#endif -#if HAS_GYRO - if (minor < 15) { - ao_config.accel_zero_along = 0; - ao_config.accel_zero_across = 0; - ao_config.accel_zero_through = 0; - - /* Reset the main accel offsets to force - * re-calibration - */ - ao_config.accel_plus_g = 0; - ao_config.accel_minus_g = 0; - } -#endif - ao_config.minor = AO_CONFIG_MINOR; - ao_config_dirty = 1; - } -#if HAS_RADIO -#if HAS_FORCE_FREQ - if (ao_force_freq) { - ao_config.frequency = 434550; - ao_config.radio_cal = ao_radio_cal; - ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN), - sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); - } -#endif - ao_config_set_radio(); -#endif - ao_config_loaded = 1; -} - -void -_ao_config_edit_start(void) -{ - ao_mutex_get(&ao_config_mutex); - _ao_config_get(); -} - -void -_ao_config_edit_finish(void) -{ - ao_config_dirty = 1; - ao_mutex_put(&ao_config_mutex); -} - -void -ao_config_get(void) -{ - _ao_config_edit_start(); - ao_mutex_put(&ao_config_mutex); -} - -void -ao_config_callsign_show(void) -{ - printf ("Callsign: \"%s\"\n", ao_config.callsign); -} - -void -ao_config_callsign_set(void) __reentrant -{ - uint8_t c; - static __xdata char callsign[AO_MAX_CALLSIGN + 1]; - - ao_xmemset(callsign, '\0', sizeof callsign); - ao_cmd_white(); - c = 0; - while (ao_cmd_lex_c != '\n') { - if (c < AO_MAX_CALLSIGN) - callsign[c++] = ao_cmd_lex_c; - else - ao_cmd_status = ao_cmd_lex_error; - ao_cmd_lex(); - } - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_xmemcpy(&ao_config.callsign, &callsign, - AO_MAX_CALLSIGN + 1); - _ao_config_edit_finish(); -} - -#if HAS_RADIO - -void -ao_config_frequency_show(void) __reentrant -{ - printf("Frequency: %ld\n", - ao_config.frequency); -} - -void -ao_config_frequency_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.frequency = ao_cmd_lex_u32; - ao_config_set_radio(); - _ao_config_edit_finish(); -#if HAS_RADIO_RECV - ao_radio_recv_abort(); -#endif -} -#endif - -#if HAS_FLIGHT - -void -ao_config_main_deploy_show(void) __reentrant -{ - printf("Main deploy: %d meters\n", - ao_config.main_deploy); -} - -void -ao_config_main_deploy_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.main_deploy = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#if HAS_ACCEL -void -ao_config_accel_calibrate_show(void) __reentrant -{ - printf("Accel cal +1g: %d -1g: %d\n", - ao_config.accel_plus_g, ao_config.accel_minus_g); -#if HAS_GYRO - printf ("IMU cal along %d across %d through %d\n", - ao_config.accel_zero_along, - ao_config.accel_zero_across, - ao_config.accel_zero_through); -#endif -} - -#define ACCEL_CALIBRATE_SAMPLES 1024 -#define ACCEL_CALIBRATE_SHIFT 10 - -#if HAS_GYRO -static int16_t accel_cal_along; -static int16_t accel_cal_across; -static int16_t accel_cal_through; -#endif - -static int16_t -ao_config_accel_calibrate_auto(char *orientation) __reentrant -{ - uint16_t i; - int32_t accel_total; - uint8_t cal_data_ring; -#if HAS_GYRO - int32_t accel_along_total = 0; - int32_t accel_across_total = 0; - int32_t accel_through_total = 0; -#endif - - printf("Orient antenna %s and press a key...", orientation); - flush(); - (void) getchar(); - puts("\r\n"); flush(); - puts("Calibrating..."); flush(); - i = ACCEL_CALIBRATE_SAMPLES; - accel_total = 0; - cal_data_ring = ao_sample_data; - while (i) { - ao_sleep(DATA_TO_XDATA(&ao_sample_data)); - while (i && cal_data_ring != ao_sample_data) { - accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]); -#if HAS_GYRO - accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]); - accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]); - accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]); -#endif - cal_data_ring = ao_data_ring_next(cal_data_ring); - i--; - } - } -#if HAS_GYRO - accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT; - accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT; - accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT; -#endif - return accel_total >> ACCEL_CALIBRATE_SHIFT; -} - -void -ao_config_accel_calibrate_set(void) __reentrant -{ - int16_t up, down; -#if HAS_GYRO - int16_t accel_along_up = 0, accel_along_down = 0; - int16_t accel_across_up = 0, accel_across_down = 0; - int16_t accel_through_up = 0, accel_through_down = 0; -#endif - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - if (ao_cmd_lex_i == 0) { - up = ao_config_accel_calibrate_auto("up"); -#if HAS_GYRO - accel_along_up = accel_cal_along; - accel_across_up = accel_cal_across; - accel_through_up = accel_cal_through; -#endif - down = ao_config_accel_calibrate_auto("down"); -#if HAS_GYRO - accel_along_down = accel_cal_along; - accel_across_down = accel_cal_across; - accel_through_down = accel_cal_through; -#endif - } else { - up = ao_cmd_lex_i; - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - down = ao_cmd_lex_i; - } - if (up >= down) { - printf("Invalid accel: up (%d) down (%d)\n", - up, down); - return; - } - _ao_config_edit_start(); - ao_config.accel_plus_g = up; - ao_config.accel_minus_g = down; -#if HAS_GYRO - if (ao_cmd_lex_i == 0) { - ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2; - ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2; - ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2; - } -#endif - _ao_config_edit_finish(); -} -#endif /* HAS_ACCEL */ - -void -ao_config_apogee_delay_show(void) __reentrant -{ - printf("Apogee delay: %d seconds\n", - ao_config.apogee_delay); -} - -void -ao_config_apogee_delay_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.apogee_delay = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -void -ao_config_apogee_lockout_show(void) __reentrant -{ - printf ("Apogee lockout: %d seconds\n", - ao_config.apogee_lockout); -} - -void -ao_config_apogee_lockout_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.apogee_lockout = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#endif /* HAS_FLIGHT */ - -#if HAS_RADIO -void -ao_config_radio_cal_show(void) __reentrant -{ - printf("Radio cal: %ld\n", ao_config.radio_cal); -} - -void -ao_config_radio_cal_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.radio_cal = ao_cmd_lex_u32; - ao_config_set_radio(); - _ao_config_edit_finish(); -} -#endif - -#if HAS_LOG -void -ao_config_log_show(void) __reentrant -{ - printf("Max flight log: %d kB\n", (int16_t) (ao_config.flight_log_max >> 10)); -} - -void -ao_config_log_set(void) __reentrant -{ - uint16_t block = (uint16_t) (ao_storage_block >> 10); - uint16_t log_max = (uint16_t) (ao_storage_log_max >> 10); - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - if (ao_log_present()) - printf("Storage must be empty before changing log size\n"); - else if (block > 1024 && (ao_cmd_lex_i & (block - 1))) - printf("Flight log size must be multiple of %d kB\n", block); - else if (ao_cmd_lex_i > log_max) - printf("Flight log max %d kB\n", log_max); - else { - _ao_config_edit_start(); - ao_config.flight_log_max = (uint32_t) ao_cmd_lex_i << 10; - _ao_config_edit_finish(); - } -} -#endif /* HAS_LOG */ - -#if HAS_IGNITE -void -ao_config_ignite_mode_show(void) __reentrant -{ - printf("Ignite mode: %d\n", ao_config.ignite_mode); -} - -void -ao_config_ignite_mode_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.ignite_mode = ao_cmd_lex_i; - _ao_config_edit_finish(); -} -#endif - -#if HAS_ACCEL -void -ao_config_pad_orientation_show(void) __reentrant -{ - printf("Pad orientation: %d\n", ao_config.pad_orientation); -} - -#ifndef AO_ACCEL_INVERT -#define AO_ACCEL_INVERT 0x7fff -#endif - -void -ao_config_pad_orientation_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_cmd_lex_i &= 1; - if (ao_config.pad_orientation != ao_cmd_lex_i) { - int16_t t; - t = ao_config.accel_plus_g; - ao_config.accel_plus_g = AO_ACCEL_INVERT - ao_config.accel_minus_g; - ao_config.accel_minus_g = AO_ACCEL_INVERT - t; - } - ao_config.pad_orientation = ao_cmd_lex_i; - _ao_config_edit_finish(); -} -#endif - -#if HAS_RADIO -void -ao_config_radio_enable_show(void) __reentrant -{ - printf("Radio enable: %d\n", ao_config.radio_enable); -} - -void -ao_config_radio_enable_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.radio_enable = ao_cmd_lex_i; - _ao_config_edit_finish(); -} -#endif /* HAS_RADIO */ - -#if HAS_AES - -__xdata uint8_t ao_config_aes_seq = 1; - -void -ao_config_key_show(void) __reentrant -{ - uint8_t i; - printf("AES key: "); - for (i = 0; i < AO_AES_LEN; i++) - printf ("%02x", ao_config.aes_key[i]); - printf("\n"); -} - -void -ao_config_key_set(void) __reentrant -{ - uint8_t i; - - _ao_config_edit_start(); - for (i = 0; i < AO_AES_LEN; i++) { - ao_cmd_hexbyte(); - if (ao_cmd_status != ao_cmd_success) - break; - ao_config.aes_key[i] = ao_cmd_lex_i; - } - ++ao_config_aes_seq; - _ao_config_edit_finish(); -} -#endif - -#if HAS_APRS - -void -ao_config_aprs_show(void) -{ - printf ("APRS interval: %d\n", ao_config.aprs_interval); -} - -void -ao_config_aprs_set(void) -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.aprs_interval = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#endif /* HAS_APRS */ - -#if HAS_RADIO_AMP - -void -ao_config_radio_amp_show(void) -{ - printf ("Radio amp setting: %d\n", ao_config.radio_amp); -} - -void -ao_config_radio_amp_set(void) -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.radio_amp = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#endif - -#if HAS_RADIO_POWER - -void -ao_config_radio_power_show(void) -{ - printf ("Radio power setting: %d\n", ao_config.radio_power); -} - -void -ao_config_radio_power_set(void) -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.radio_power = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#endif - -struct ao_config_var { - __code char *str; - void (*set)(void) __reentrant; - void (*show)(void) __reentrant; -}; - -static void -ao_config_help(void) __reentrant; - -static void -ao_config_show(void) __reentrant; - -#if HAS_EEPROM -static void -ao_config_save(void) __reentrant; -#endif - -__code struct ao_config_var ao_config_vars[] = { -#if HAS_FLIGHT - { "m \0Main deploy (m)", - ao_config_main_deploy_set, ao_config_main_deploy_show, }, - { "d \0Apogee delay (s)", - ao_config_apogee_delay_set, ao_config_apogee_delay_show }, - { "L \0Apogee detect lockout (s)", - ao_config_apogee_lockout_set, ao_config_apogee_lockout_show, }, -#endif /* HAS_FLIGHT */ -#if HAS_RADIO - { "F \0Frequency (kHz)", - ao_config_frequency_set, ao_config_frequency_show }, - { "c \0Callsign (8 char max)", - ao_config_callsign_set, ao_config_callsign_show }, - { "e <0 disable, 1 enable>\0Enable telemetry and RDF", - ao_config_radio_enable_set, ao_config_radio_enable_show }, - { "f \0Radio calib (cal = rf/(xtal/2^16))", - ao_config_radio_cal_set, ao_config_radio_cal_show }, -#if HAS_RADIO_POWER - { "p \0Radio power setting (0-255)", - ao_config_radio_power_set, ao_config_radio_power_show }, -#endif -#if HAS_RADIO_AMP - { "d \0Radio amplifier setting (0-3)", - ao_config_radio_amp_set, ao_config_radio_amp_show }, -#endif -#endif /* HAS_RADIO */ -#if HAS_ACCEL - { "a <+g> <-g>\0Accel calib (0 for auto)", - ao_config_accel_calibrate_set,ao_config_accel_calibrate_show }, - { "o <0 antenna up, 1 antenna down>\0Set pad orientation", - ao_config_pad_orientation_set,ao_config_pad_orientation_show }, -#endif /* HAS_ACCEL */ -#if HAS_LOG - { "l \0Flight log size (kB)", - ao_config_log_set, ao_config_log_show }, -#endif -#if HAS_IGNITE - { "i <0 dual, 1 apogee, 2 main>\0Set igniter mode", - ao_config_ignite_mode_set, ao_config_ignite_mode_show }, -#endif -#if HAS_AES - { "k <32 hex digits>\0Set AES encryption key", - ao_config_key_set, ao_config_key_show }, -#endif -#if AO_PYRO_NUM - { "P \0Configure pyro channels", - ao_pyro_set, ao_pyro_show }, -#endif -#if HAS_APRS - { "A \0APRS packet interval (0 disable)", - ao_config_aprs_set, ao_config_aprs_show }, -#endif - { "s\0Show", - ao_config_show, 0 }, -#if HAS_EEPROM - { "w\0Write to eeprom", - ao_config_save, 0 }, -#endif - { "?\0Help", - ao_config_help, 0 }, - { 0, 0, 0 } -}; - -void -ao_config_set(void) -{ - char c; - uint8_t cmd; - - ao_cmd_white(); - c = ao_cmd_lex_c; - ao_cmd_lex(); - for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) - if (ao_config_vars[cmd].str[0] == c) { - (*ao_config_vars[cmd].set)(); - return; - } - ao_cmd_status = ao_cmd_syntax_error; -} - -static void -ao_config_help(void) __reentrant -{ - uint8_t cmd; - for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) - printf("%-20s %s\n", - ao_config_vars[cmd].str, - ao_config_vars[cmd].str+1+ - strlen(ao_config_vars[cmd].str)); -} - -static void -ao_config_show(void) __reentrant -{ - uint8_t cmd; - ao_config_get(); - printf("Config version: %d.%d\n", - ao_config.major, ao_config.minor); - for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) - if (ao_config_vars[cmd].show) - (*ao_config_vars[cmd].show)(); -#if HAS_MS5607 - ao_ms5607_info(); -#endif -} - -#if HAS_EEPROM -static void -ao_config_save(void) __reentrant -{ - uint8_t saved = 0; - ao_mutex_get(&ao_config_mutex); - if (ao_config_dirty) { - _ao_config_put(); - ao_config_dirty = 0; - saved = 1; - } - ao_mutex_put(&ao_config_mutex); - if (saved) - puts("Saved"); - else - puts("Nothing to save"); -} -#endif - -__code struct ao_cmds ao_config_cmds[] = { - { ao_config_set, "c \0Set config (? for help, s to show)" }, - { 0, NULL }, -}; - -void -ao_config_init(void) -{ - ao_cmd_register(&ao_config_cmds[0]); -} diff --git a/src/core/ao_config.h b/src/core/ao_config.h deleted file mode 100644 index e101af8e..00000000 --- a/src/core/ao_config.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_CONFIG_H_ -#define _AO_CONFIG_H_ - -#ifndef USE_STORAGE_CONFIG -#define USE_STORAGE_CONFIG 1 -#endif - -#ifndef USE_EEPROM_CONFIG -#define USE_EEPROM_CONFIG 0 -#endif - -#if USE_STORAGE_CONFIG - -#include - -#define ao_config_setup() ao_storage_setup() -#define ao_config_erase() ao_storage_erase(ao_storage_config) -#define ao_config_write(pos,bytes, len) ao_storage_write(ao_storage_config+(pos), bytes, len) -#define ao_config_read(pos,bytes, len) ao_storage_read(ao_storage_config+(pos), bytes, len) -#define ao_config_flush() ao_storage_flush() - -#endif - -#if USE_EEPROM_CONFIG - -#include - -#define ao_config_setup() -#define ao_config_erase() -#define ao_config_write(pos,bytes, len) ao_eeprom_write(pos, bytes, len) -#define ao_config_read(pos,bytes, len) ao_eeprom_read(pos, bytes, len) -#define ao_config_flush() - -#endif - -#endif /* _AO_CONFIG_H_ */ diff --git a/src/core/ao_convert.c b/src/core/ao_convert.c deleted file mode 100644 index aa9b5f48..00000000 --- a/src/core/ao_convert.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST) -#include "ao.h" -#endif - -static const int16_t altitude_table[] = { -#include "altitude.h" -}; - -#define ALT_FRAC_SCALE (1 << ALT_FRAC_BITS) -#define ALT_FRAC_MASK (ALT_FRAC_SCALE - 1) - -int16_t -ao_pres_to_altitude(int16_t pres) __reentrant -{ - uint8_t o; - int16_t part; - - if (pres < 0) - pres = 0; - o = pres >> ALT_FRAC_BITS; - part = pres & ALT_FRAC_MASK; - - return ((int32_t) altitude_table[o] * (ALT_FRAC_SCALE - part) + - (int32_t) altitude_table[o+1] * part + (ALT_FRAC_SCALE >> 1)) >> ALT_FRAC_BITS; -} - -#if AO_NEED_ALTITUDE_TO_PRES -int16_t -ao_altitude_to_pres(int16_t alt) __reentrant -{ - int16_t span, sub_span; - uint8_t l, h, m; - int32_t pres; - - l = 0; - h = NALT - 1; - while ((h - l) != 1) { - m = (l + h) >> 1; - if (altitude_table[m] < alt) - h = m; - else - l = m; - } - span = altitude_table[l] - altitude_table[h]; - sub_span = altitude_table[l] - alt; - pres = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_FRAC_BITS) + (span >> 1)) / span; - if (pres > 32767) - pres = 32767; - if (pres < 0) - pres = 0; - return (int16_t) pres; -} -#endif - -#if 0 -int16_t -ao_temp_to_dC(int16_t temp) __reentrant -{ - int16_t ret; - - /* Output voltage at 0°C = 0.755V - * Coefficient = 0.00247V/°C - * Reference voltage = 1.25V - * - * temp = ((value / 32767) * 1.25 - 0.755) / 0.00247 - * = (value - 19791.268) / 32768 * 1.25 / 0.00247 - * ≃ (value - 19791) * 1012 / 65536 - */ - ret = ((temp - 19791) * 1012L) >> 16; - return ret; -} -#endif diff --git a/src/core/ao_convert_pa.c b/src/core/ao_convert_pa.c deleted file mode 100644 index fe6e0ef6..00000000 --- a/src/core/ao_convert_pa.c +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST) -#include "ao.h" -#endif - -#ifndef AO_CONST_ATTRIB -#define AO_CONST_ATTRIB -#endif - -static const alt_t altitude_table[] AO_CONST_ATTRIB = { -#include "altitude-pa.h" -}; - -#ifndef FETCH_ALT -#define FETCH_ALT(o) altitude_table[o] -#endif - -#define ALT_SCALE (1 << ALT_SHIFT) -#define ALT_MASK (ALT_SCALE - 1) - -alt_t -ao_pa_to_altitude(int32_t pa) -{ - int16_t o; - int16_t part; - int32_t low, high; - - if (pa < 0) - pa = 0; - if (pa > 120000L) - pa = 120000L; - o = pa >> ALT_SHIFT; - part = pa & ALT_MASK; - - low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part); - high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); - return (low + high) >> ALT_SHIFT; -} - -#ifdef AO_CONVERT_TEST -int32_t -ao_altitude_to_pa(int32_t alt) -{ - int32_t span, sub_span; - uint16_t l, h, m; - int32_t pa; - - l = 0; - h = NALT - 1; - while ((h - l) != 1) { - m = (l + h) >> 1; - if (altitude_table[m] < alt) - h = m; - else - l = m; - } - span = altitude_table[l] - altitude_table[h]; - sub_span = altitude_table[l] - alt; - pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span; - if (pa > 120000) - pa = 120000; - if (pa < 0) - pa = 0; - return pa; -} -#endif diff --git a/src/core/ao_convert_pa_test.c b/src/core/ao_convert_pa_test.c deleted file mode 100644 index 7d5b1922..00000000 --- a/src/core/ao_convert_pa_test.c +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#define AO_CONVERT_TEST -typedef int32_t alt_t; -#include "ao_host.h" -#include "ao_convert_pa.c" - -#define STEP_P 1 -#define STEP_A 1 - -static inline int i_abs(int i) { return i < 0 ? -i : i; } - -int -main (int argc, char **argv) -{ - int i; - int32_t p_to_a, p_to_a_to_p; - int32_t a_to_p, a_to_p_to_a; - int max_p_error = 0, max_p_error_p = -1; - int max_a_error = 0, max_a_error_a = -1; - int p_error; - int a_error; - int ret = 0; - - for (i = 0; i < 120000 + STEP_P; i += STEP_P) { - if (i > 120000) - i = 120000; - p_to_a = ao_pa_to_altitude(i); - p_to_a_to_p = ao_altitude_to_pa(p_to_a); - p_error = i_abs(p_to_a_to_p - i); - if (p_error > max_p_error) { - max_p_error = p_error; - max_p_error_p = i; - } -// printf ("pa %d alt %d pa %d\n", -// i, p_to_a, p_to_a_to_p); - } - for (i = -1450; i < 40000 + STEP_A; i += STEP_A) { - a_to_p = ao_altitude_to_pa(i); - a_to_p_to_a = ao_pa_to_altitude(a_to_p); - a_error = i_abs(a_to_p_to_a - i); - if (a_error > max_a_error) { - max_a_error = a_error; - max_a_error_a = i; - } -// printf ("alt %d pa %d alt %d\n", -// i, a_to_p, a_to_p_to_a); - } - if (max_p_error > 2) { - printf ("max p error %d at %d\n", max_p_error, - max_p_error_p); - ret++; - } - if (max_a_error > 1) { - printf ("max a error %d at %d\n", max_a_error, - max_a_error_a); - ret++; - } - return ret; -} diff --git a/src/core/ao_convert_test.c b/src/core/ao_convert_test.c deleted file mode 100644 index 87e76841..00000000 --- a/src/core/ao_convert_test.c +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Copyright © 2010 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#define AO_CONVERT_TEST -#define AO_NEED_ALTITUDE_TO_PRES 1 -#include "ao_host.h" -#include "ao_convert.c" - -#define STEP 1 - -static inline int i_abs(int i) { return i < 0 ? -i : i; } - -int main (int argc, char **argv) -{ - int i; - int16_t p_to_a, p_to_a_to_p; - int16_t a_to_p, a_to_p_to_a; - int max_p_error = 0, max_p_error_p = -1; - int max_a_error = 0, max_a_error_a = -1; - int p_error; - int a_error; - int ret = 0; - - for (i = 0; i < 32767 + STEP; i += STEP) { - if (i > 32767) - i = 32767; - p_to_a = ao_pres_to_altitude(i); - p_to_a_to_p = ao_altitude_to_pres(p_to_a); - p_error = i_abs(p_to_a_to_p - i); - if (p_error > max_p_error) { - max_p_error = p_error; - max_p_error_p = i; - } -// printf ("pres %d alt %d pres %d\n", -// i, p_to_a, p_to_a_to_p); - } - for (i = -1578; i < 15835 + STEP; i += STEP) { - if (i > 15835) - i = 15835; - a_to_p = ao_altitude_to_pres(i); - a_to_p_to_a = ao_pres_to_altitude(a_to_p); - a_error = i_abs(a_to_p_to_a - i); - if (a_error > max_a_error) { - max_a_error = a_error; - max_a_error_a = i; - } -// printf ("alt %d pres %d alt %d\n", -// i, a_to_p, a_to_p_to_a); - } - if (max_p_error > 2) { - printf ("max p error %d at %d\n", max_p_error, - max_p_error_p); - ret++; - } - if (max_a_error > 1) { - printf ("max a error %d at %d\n", max_a_error, - max_a_error_a); - ret++; - } - return ret; -} diff --git a/src/core/ao_convert_volt.c b/src/core/ao_convert_volt.c deleted file mode 100644 index 8556d423..00000000 --- a/src/core/ao_convert_volt.c +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright © 2014 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -#define scale(v,p,m) ((int32_t) (v) * (AO_ADC_REFERENCE_DV * ((p) + (m))) / (AO_ADC_MAX * (m))) - -int16_t -ao_battery_decivolt(int16_t adc) -{ - return scale(adc, AO_BATTERY_DIV_PLUS, AO_BATTERY_DIV_MINUS); -} - -int16_t -ao_ignite_decivolt(int16_t adc) -{ - return scale(adc, AO_IGNITE_DIV_PLUS, AO_IGNITE_DIV_MINUS); -} - diff --git a/src/core/ao_data.c b/src/core/ao_data.c deleted file mode 100644 index 6a3d02a1..00000000 --- a/src/core/ao_data.c +++ /dev/null @@ -1,36 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include - -volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; -volatile __data uint8_t ao_data_head; -volatile __data uint8_t ao_data_present; - -#ifndef ao_data_count -void -ao_data_get(__xdata struct ao_data *packet) -{ -#if HAS_FLIGHT - uint8_t i = ao_data_ring_prev(ao_sample_data); -#else - uint8_t i = ao_data_ring_prev(ao_data_head); -#endif - memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data)); -} -#endif diff --git a/src/core/ao_data.h b/src/core/ao_data.h deleted file mode 100644 index c4b062fd..00000000 --- a/src/core/ao_data.h +++ /dev/null @@ -1,338 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_DATA_H_ -#define _AO_DATA_H_ - -#define GRAVITY 9.80665 - -#if HAS_ADC -#define AO_DATA_ADC (1 << 0) -#else -#define AO_DATA_ADC 0 -#endif - -#if HAS_MS5607 -#include -#define AO_DATA_MS5607 (1 << 1) -#else -#define AO_DATA_MS5607 0 -#endif - -#if HAS_MPU6000 -#include -#define AO_DATA_MPU6000 (1 << 2) -#else -#define AO_DATA_MPU6000 0 -#endif - -#if HAS_HMC5883 -#include -#define AO_DATA_HMC5883 (1 << 3) -#else -#define AO_DATA_HMC5883 0 -#endif - -#if HAS_MMA655X -#include -#define AO_DATA_MMA655X (1 << 4) -#else -#define AO_DATA_MMA655X 0 -#endif - -#ifdef AO_DATA_RING - -#define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X) - -struct ao_data { - uint16_t tick; -#if HAS_ADC - struct ao_adc adc; -#endif -#if HAS_MS5607 - struct ao_ms5607_sample ms5607_raw; - struct ao_ms5607_value ms5607_cooked; -#endif -#if HAS_MPU6000 - struct ao_mpu6000_sample mpu6000; -#if !HAS_MMA655X - int16_t z_accel; -#endif -#endif -#if HAS_HMC5883 - struct ao_hmc5883_sample hmc5883; -#endif -#if HAS_MMA655X - uint16_t mma655x; -#endif -}; - -#define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) -#define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) - -/* Get a copy of the last complete sample set */ -void -ao_data_get(__xdata struct ao_data *packet); - -extern volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; -extern volatile __data uint8_t ao_data_head; -extern volatile __data uint8_t ao_data_present; -extern volatile __data uint8_t ao_data_count; - -/* - * Mark a section of data as ready, check for data complete - */ -#define AO_DATA_PRESENT(bit) (ao_data_present |= (bit)) - -/* - * Wait until it is time to write a sensor sample; this is - * signaled by the timer tick - */ -#define AO_DATA_WAIT() do { \ - ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count)); \ - } while (0) - -#endif /* AO_DATA_RING */ - -#if !HAS_BARO && HAS_MS5607 - -/* Either an MS5607 or an MS5611 hooked to a SPI port - */ - -#define HAS_BARO 1 - -typedef int32_t pres_t; - -#ifndef AO_ALT_TYPE -#define AO_ALT_TYPE int32_t -#endif - -typedef AO_ALT_TYPE alt_t; - -#define ao_data_pres_cook(packet) ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked) - -#define ao_data_pres(packet) ((packet)->ms5607_cooked.pres) -#define ao_data_temp(packet) ((packet)->ms5607_cooked.temp) - -#define pres_to_altitude(p) ao_pa_to_altitude(p) - -#endif - -#if !HAS_BARO && HAS_ADC - -#define HAS_BARO 1 - -typedef int16_t pres_t; -typedef int16_t alt_t; - -#define ao_data_pres(packet) ((packet)->adc.pres) -#define ao_data_temp(packet) ((packet)->adc.temp) -#define pres_to_altitude(p) ao_pres_to_altitude(p) -#define ao_data_pres_cook(p) - -#endif - -#if !HAS_BARO -typedef int16_t alt_t; -#endif - -/* - * Need a few macros to pull data from the sensors: - * - * ao_data_accel_sample - pull raw sensor and convert to normalized values - * ao_data_accel - pull normalized value (lives in the same memory) - * ao_data_set_accel - store normalized value back in the sensor location - * ao_data_accel_invert - flip rocket ends for positive acceleration - */ - -#if HAS_ACCEL - -/* This section is for an analog accelerometer hooked to one of the ADC pins. As - * those are 5V parts, this also requires that the 5V supply be hooked to to anothe ADC - * pin so that the both can be measured to correct for changes between the 3.3V and 5V rails - */ - -typedef int16_t accel_t; -#define ao_data_accel(packet) ((packet)->adc.accel) -#define ao_data_set_accel(packet, a) ((packet)->adc.accel = (a)) -#define ao_data_accel_invert(a) (0x7fff -(a)) - -/* - * Ok, the math here is a bit tricky. - * - * ao_sample_accel: ADC output for acceleration - * ao_accel_ref: ADC output for the 5V reference. - * ao_cook_accel: Corrected acceleration value - * Vcc: 3.3V supply to the CC1111 - * Vac: 5V supply to the accelerometer - * accel: input voltage to accelerometer ADC pin - * ref: input voltage to 5V reference ADC pin - * - * - * Measured acceleration is ratiometric to Vcc: - * - * ao_sample_accel accel - * ------------ = ----- - * 32767 Vcc - * - * Measured 5v reference is also ratiometric to Vcc: - * - * ao_accel_ref ref - * ------------ = ----- - * 32767 Vcc - * - * - * ao_accel_ref = 32767 * (ref / Vcc) - * - * Acceleration is measured ratiometric to the 5V supply, - * so what we want is: - * - * ao_cook_accel accel - * ------------- = ----- - * 32767 ref - * - * - * accel Vcc - * = ----- * --- - * Vcc ref - * - * ao_sample_accel 32767 - * = ------------ * ------------ - * 32767 ao_accel_ref - * - * Multiply through by 32767: - * - * ao_sample_accel * 32767 - * ao_cook_accel = -------------------- - * ao_accel_ref - * - * Now, the tricky part. Getting this to compile efficiently - * and keeping all of the values in-range. - * - * First off, we need to use a shift of 16 instead of * 32767 as SDCC - * does the obvious optimizations for byte-granularity shifts: - * - * ao_cook_accel = (ao_sample_accel << 16) / ao_accel_ref - * - * Next, lets check our input ranges: - * - * 0 <= ao_sample_accel <= 0x7fff (singled ended ADC conversion) - * 0x7000 <= ao_accel_ref <= 0x7fff (the 5V ref value is close to 0x7fff) - * - * Plugging in our input ranges, we get an output range of 0 - 0x12490, - * which is 17 bits. That won't work. If we take the accel ref and shift - * by a bit, we'll change its range: - * - * 0xe000 <= ao_accel_ref<<1 <= 0xfffe - * - * ao_cook_accel = (ao_sample_accel << 16) / (ao_accel_ref << 1) - * - * Now the output range is 0 - 0x9248, which nicely fits in 16 bits. It - * is, however, one bit too large for our signed computations. So, we - * take the result and shift that by a bit: - * - * ao_cook_accel = ((ao_sample_accel << 16) / (ao_accel_ref << 1)) >> 1 - * - * This finally creates an output range of 0 - 0x4924. As the ADC only - * provides 11 bits of data, we haven't actually lost any precision, - * just dropped a bit of noise off the low end. - */ - -#if HAS_ACCEL_REF - -#define ao_data_accel_cook(packet) \ - ((uint16_t) ((((uint32_t) (packet)->adc.accel << 16) / ((packet)->adc.accel_ref << 1))) >> 1) - -#else - -#define ao_data_accel_cook(packet) ((packet)->adc.accel) - -#endif /* HAS_ACCEL_REF */ - -#endif /* HAS_ACCEL */ - -#if !HAS_ACCEL && HAS_MMA655X - -#define HAS_ACCEL 1 - -typedef int16_t accel_t; - -/* MMA655X is hooked up so that positive values represent negative acceleration */ - -#define AO_ACCEL_INVERT 4095 - -#define ao_data_accel(packet) ((packet)->mma655x) -#if AO_MMA655X_INVERT -#define ao_data_accel_cook(packet) (AO_ACCEL_INVERT - (packet)->mma655x) -#else -#define ao_data_accel_cook(packet) ((packet)->mma655x) -#endif -#define ao_data_set_accel(packet, accel) ((packet)->mma655x = (accel)) -#define ao_data_accel_invert(accel) (AO_ACCEL_INVERT - (accel)) - -#endif - -#if !HAS_ACCEL && HAS_MPU6000 - -#define HAS_ACCEL 1 - -#define AO_ACCEL_INVERT 0 - -typedef int16_t accel_t; - -/* MPU6000 is hooked up so that positive y is positive acceleration */ -#define ao_data_accel(packet) ((packet)->z_accel) -#define ao_data_accel_cook(packet) (-(packet)->mpu6000.accel_y) -#define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) -#define ao_data_accel_invert(a) (-(a)) - -#endif - -#if !HAS_GYRO && HAS_MPU6000 - -#define HAS_GYRO 1 - -typedef int16_t gyro_t; /* in raw sample units */ -typedef int16_t angle_t; /* in degrees */ - -/* Y axis is aligned with the direction of motion (along) */ -/* X axis is aligned in the other board axis (across) */ -/* Z axis is aligned perpendicular to the board (through) */ - -#define ao_data_along(packet) ((packet)->mpu6000.accel_y) -#define ao_data_across(packet) ((packet)->mpu6000.accel_x) -#define ao_data_through(packet) ((packet)->mpu6000.accel_z) - -#define ao_data_roll(packet) ((packet)->mpu6000.gyro_y) -#define ao_data_pitch(packet) ((packet)->mpu6000.gyro_x) -#define ao_data_yaw(packet) ((packet)->mpu6000.gyro_z) - -#endif - -#if !HAS_MAG && HAS_HMC5883 - -#define HAS_MAG 1 - -typedef int16_t ao_mag_t; /* in raw sample units */ - -#define ao_data_mag_along(packet) ((packet)->hmc5883.x) -#define ao_data_mag_across(packet) ((packet)->hmc5883.y) -#define ao_data_mag_through(packet) ((packet)->hmc5883.z) - -#endif - -#endif /* _AO_DATA_H_ */ diff --git a/src/core/ao_dbg.h b/src/core/ao_dbg.h deleted file mode 100644 index 181e6ec2..00000000 --- a/src/core/ao_dbg.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_DBG_H_ -#define _AO_DBG_H_ - -/* - * ao_dbg.c - * - * debug another telemetrum board - */ - -/* Send a byte to the dbg target */ -void -ao_dbg_send_byte(uint8_t byte); - -/* Receive a byte from the dbg target */ -uint8_t -ao_dbg_recv_byte(void); - -/* Start a bulk transfer to/from dbg target memory */ -void -ao_dbg_start_transfer(uint16_t addr); - -/* End a bulk transfer to/from dbg target memory */ -void -ao_dbg_end_transfer(void); - -/* Write a byte to dbg target memory */ -void -ao_dbg_write_byte(uint8_t byte); - -/* Read a byte from dbg target memory */ -uint8_t -ao_dbg_read_byte(void); - -/* Enable dbg mode, switching use of the pins */ -void -ao_dbg_debug_mode(void); - -/* Reset the dbg target */ -void -ao_dbg_reset(void); - -void -ao_dbg_init(void); - -#endif /* _AO_DBG_H_ */ diff --git a/src/core/ao_debounce.c b/src/core/ao_debounce.c deleted file mode 100644 index b9d67729..00000000 --- a/src/core/ao_debounce.c +++ /dev/null @@ -1,163 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include -#include - -static uint8_t ao_debounce_initialized; -static uint8_t ao_debounce_running; -static struct ao_debounce *ao_debounce; - -static uint8_t values[64]; -static uint8_t n; - -#define d_step(n) (((n) + 1) & 63) - -static void -_ao_debounce_set(struct ao_debounce *debounce, uint8_t value) -{ - if (value != debounce->value) { - values[n] = value; - n = (n + 1) & 63; - debounce->value = value; - debounce->_set(debounce, value); - } - _ao_debounce_stop(debounce); -} - -void -ao_debounce_dump(void) -{ - uint8_t s; - - for (s = 0; s < n; s++) { - printf ("%d: %d\n", - s, values[s]); - } - n = 0; -} - -/* - * Get the current value, set the result when we've - * reached the debounce count limit - */ -static void -_ao_debounce_check(struct ao_debounce *debounce) -{ - uint8_t next = debounce->_get(debounce); - - if (next == debounce->current) { - if (debounce->count < debounce->hold) { - if (++debounce->count == debounce->hold) - _ao_debounce_set(debounce, debounce->current); - } - } else { - debounce->count = 0; - debounce->current = next; - } -} - -static void -_ao_debounce_isr(void) -{ - struct ao_debounce *debounce, *next; - - for (debounce = ao_debounce; debounce; debounce = next) { - next = debounce->next; - _ao_debounce_check(debounce); - } -} - -static void -ao_debounce_on(void) -{ - ao_fast_timer_on(_ao_debounce_isr); -} - -static void -ao_debounce_off(void) -{ - ao_fast_timer_off(_ao_debounce_isr); -} - -/* - * Start monitoring one pin - */ -void -_ao_debounce_start(struct ao_debounce *debounce) -{ - uint32_t m; - - m = ao_arch_irqsave(); - if (!debounce->running) { - debounce->running = 1; - - /* Reset the counter */ - debounce->count = 0; - - /* Link into list */ - debounce->next = ao_debounce; - ao_debounce = debounce; - - /* Make sure the timer is running */ - if (!ao_debounce_running++) - ao_debounce_on(); - - /* And go check the current value */ - _ao_debounce_check(debounce); - } - ao_arch_irqrestore(m); -} - -/* - * Stop monitoring one pin - */ -void -_ao_debounce_stop(struct ao_debounce *debounce) -{ - struct ao_debounce **prev; - uint32_t m; - - m = ao_arch_irqsave(); - if (debounce->running) { - debounce->running = 0; - - /* Unlink */ - for (prev = &ao_debounce; (*prev); prev = &((*prev)->next)) { - if (*prev == debounce) { - *prev = debounce->next; - break; - } - } - debounce->next = NULL; - - /* Turn off the timer if possible */ - if (!--ao_debounce_running) - ao_debounce_off(); - } - ao_arch_irqrestore(m); -} - -void -ao_debounce_init(void) -{ - if (ao_debounce_initialized) - return; - ao_debounce_initialized = 1; - ao_fast_timer_init(); -} diff --git a/src/core/ao_debounce.h b/src/core/ao_debounce.h deleted file mode 100644 index 19c620f5..00000000 --- a/src/core/ao_debounce.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_DEBOUNCE_H_ -#define _AO_DEBOUNCE_H_ - -struct ao_debounce { - struct ao_debounce *next; - - /* time that pin value must be stable before accepting */ - uint8_t hold; - - /* last value reported to app; don't report it twice */ - uint8_t value; - - /* current value received from pins */ - uint8_t current; - - /* current count of intervals pin value has been stable */ - uint8_t count; - - /* This pin is running */ - uint8_t running; - - /* Get the current pin value */ - uint8_t (*_get)(struct ao_debounce *debounce); - - /* The stable value has changed */ - void (*_set)(struct ao_debounce *debounce, uint8_t value); -}; - -static inline void -ao_debounce_config(struct ao_debounce *debounce, - uint8_t (*_get)(struct ao_debounce *debounce), - void (*_set)(struct ao_debounce *debounce, uint8_t value), - uint8_t hold) -{ - debounce->next = 0; - debounce->hold = hold; - debounce->value = 0xff; - debounce->current = 0xff; - debounce->count = 0; - debounce->running = 0; - debounce->_get = _get; - debounce->_set = _set; -} - -void -_ao_debounce_start(struct ao_debounce *debounce); - -void -_ao_debounce_stop(struct ao_debounce *debounce); - -void -ao_debounce_init(void); - -void -ao_debounce_dump(void); - -#endif /* _AO_DEBOUNCE_H_ */ diff --git a/src/core/ao_ee_fake.c b/src/core/ao_ee_fake.c deleted file mode 100644 index 7fcfcab0..00000000 --- a/src/core/ao_ee_fake.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -/* - * For hardware without eeprom, the config code still - * wants to call these functions - */ -uint8_t -ao_ee_write_config(uint8_t *buf, uint16_t len) __reentrant -{ - (void) buf; - (void) len; - return 1; -} - -uint8_t -ao_ee_read_config(uint8_t *buf, uint16_t len) __reentrant -{ - ao_xmemset(buf, '\0', len); - return 1; -} diff --git a/src/core/ao_eeprom.h b/src/core/ao_eeprom.h deleted file mode 100644 index 915522bf..00000000 --- a/src/core/ao_eeprom.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_EEPROM_H_ -#define _AO_EEPROM_H_ - -extern const ao_pos_t ao_eeprom_total; - -/* - * Write to eeprom - */ - -uint8_t -ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len); - -/* - * Read from eeprom - */ -uint8_t -ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len); - -/* - * Initialize eeprom - */ - -void -ao_eeprom_init(void); - -#endif /* _AO_EEPROM_H_ */ diff --git a/src/core/ao_fec.h b/src/core/ao_fec.h deleted file mode 100644 index 618756c1..00000000 --- a/src/core/ao_fec.h +++ /dev/null @@ -1,84 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_FEC_H_ -#define _AO_FEC_H_ - -#include - -#define AO_FEC_CRC_INIT 0xffff -#define AO_FEC_TRELLIS_TERMINATOR 0x0b -#define AO_FEC_PREPARE_EXTRA 4 - -extern const uint8_t ao_fec_whiten_table[]; - -#if AO_FEC_DEBUG -void -ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name); -#endif - -static inline uint16_t -ao_fec_crc_byte(uint8_t byte, uint16_t crc) -{ - uint8_t bit; - - for (bit = 0; bit < 8; bit++) { - if (((crc & 0x8000) >> 8) ^ (byte & 0x80)) - crc = (crc << 1) ^ 0x8005; - else - crc = (crc << 1); - byte <<= 1; - } - return crc; -} - -uint16_t -ao_fec_crc(const uint8_t *bytes, uint8_t len); - -/* - * 'len' is the length of the original data; 'bytes' - * must be four bytes longer than that, and the first - * two after 'len' must be the received crc - */ -uint8_t -ao_fec_check_crc(const uint8_t *bytes, uint8_t len); - -/* - * Compute CRC, whiten, convolve and interleave data. 'out' must be (len + 4) * 2 bytes long - */ -uint8_t -ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out); - -/* - * Decode data. 'in' is one byte per bit, soft decision - * 'out' must be len/8 bytes long - */ - -#define AO_FEC_DECODE_BLOCK (32) /* callback must return multiples of this many bits */ - -#define AO_FEC_DECODE_CRC_OK 0x80 /* stored in out[out_len-1] */ - -uint8_t -ao_fec_decode(const uint8_t *in, uint16_t in_len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void)); - -/* - * Interleave data packed in bytes. 'out' must be 'len' bytes long. - */ -uint16_t -ao_fec_interleave_bytes(uint8_t *in, uint16_t len, uint8_t *out); - -#endif /* _AO_FEC_H_ */ diff --git a/src/core/ao_fec_rx.c b/src/core/ao_fec_rx.c deleted file mode 100644 index c4f5559a..00000000 --- a/src/core/ao_fec_rx.c +++ /dev/null @@ -1,318 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include - -#ifdef TELEMEGA -#include -#endif - -#if AO_PROFILE -#include - -uint32_t ao_fec_decode_start, ao_fec_decode_end; -#endif - -/* - * byte order repeats through 3 2 1 0 - * - * bit-pair order repeats through - * - * 1/0 3/2 5/4 7/6 - * - * So, the over all order is: - * - * 3,1/0 2,1/0 1,1/0 0,1/0 - * 3,3/2 2,3/2 1,3/2 0,3/2 - * 3,5/4 2,5/4 1,5/4 0,5/4 - * 3,7/6 2,7/6 1,7/6 0,7/6 - * - * The raw bit order is thus - * - * 1e/1f 16/17 0e/0f 06/07 - * 1c/1d 14/15 0c/0d 04/05 - * 1a/1b 12/13 0a/0b 02/03 - * 18/19 10/11 08/09 00/01 - */ - -static const uint8_t ao_interleave_order[] = { - 0x1e, 0x16, 0x0e, 0x06, - 0x1c, 0x14, 0x0c, 0x04, - 0x1a, 0x12, 0x0a, 0x02, - 0x18, 0x10, 0x08, 0x00 -}; - -static inline uint16_t ao_interleave_index(uint16_t i) { - return (i & ~0x1e) | ao_interleave_order[(i & 0x1e) >> 1]; -} - -#define NUM_STATE 8 -#define NUM_HIST 24 - -typedef uint32_t bits_t; - -#define V_0 0xff -#define V_1 0x00 - -/* - * These are just the 'zero' states; the 'one' states mirror them - */ -static const uint8_t ao_fec_decode_table[NUM_STATE*2] = { - V_0, V_0, /* 000 */ - V_0, V_1, /* 001 */ - V_1, V_1, /* 010 */ - V_1, V_0, /* 011 */ - V_1, V_1, /* 100 */ - V_1, V_0, /* 101 */ - V_0, V_0, /* 110 */ - V_0, V_1 /* 111 */ -}; - -static inline uint8_t -ao_next_state(uint8_t state, uint8_t bit) -{ - return ((state << 1) | bit) & 0x7; -} - -/* - * 'in' is 8-bits per symbol soft decision data - * 'len' is input byte length. 'out' must be - * 'len'/16 bytes long - */ - -uint8_t -ao_fec_decode(const uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void)) -{ - static uint32_t cost[2][NUM_STATE]; /* path cost */ - static bits_t bits[2][NUM_STATE]; /* save bits to quickly output them */ - - uint16_t i; /* input byte index */ - uint16_t b; /* encoded symbol index (bytes/2) */ - uint16_t o; /* output bit index */ - uint8_t p; /* previous cost/bits index */ - uint8_t n; /* next cost/bits index */ - uint8_t state; /* state index */ - const uint8_t *whiten = ao_fec_whiten_table; - uint16_t interleave; /* input byte array index */ - uint8_t s0, s1; - uint16_t avail; - uint16_t crc = AO_FEC_CRC_INIT; -#if AO_PROFILE - uint32_t start_tick; -#endif - - p = 0; - for (state = 0; state < NUM_STATE; state++) { - cost[0][state] = 0x7fffffff; - bits[0][state] = 0; - } - cost[0][0] = 0; - - if (callback) - avail = 0; - else - avail = len; - -#if AO_PROFILE - if (!avail) { - avail = callback(); - if (!avail) - return 0; - } - start_tick = ao_profile_tick(); -#endif - o = 0; - for (i = 0; i < len; i += 2) { - b = i/2; - n = p ^ 1; - - if (!avail) { - avail = callback(); - if (!avail) - return 0; - } - - /* Fetch one pair of input bytes, de-interleaving - * the input. - */ - interleave = ao_interleave_index(i); - s0 = in[interleave]; - s1 = in[interleave+1]; - - avail -= 2; - - /* Compute path costs and accumulate output bit path - * for each state and encoded bit value. Unrolling - * this loop is worth about > 30% performance boost. - * Decoding 76-byte remote access packets is reduced - * from 14.700ms to 9.3ms. Redoing the loop to - * directly compare the two pasts for each future state - * reduces this down to 5.7ms - */ - - /* Ok, of course this is tricky, it's optimized. - * - * First, it's important to realize that we have 8 - * states representing the combinations of the three - * most recent bits from the encoder. Flipping any - * of these three bits flips both output bits. - * - * 'state<<1' represents the target state for a new - * bit value of 0. '(state<<1)+1' represents the - * target state for a new bit value of 1. - * - * 'state' is the previous state with an oldest bit - * value of 0. 'state + 4' is the previous state with - * an oldest bit value of 1. These two states will - * either lead to 'state<<1' or '(state<<1)+1', depending - * on whether the next encoded bit was a zero or a one. - * - * m0 and m1 are the cost of coming to 'state<<1' from - * one of the two possible previous states 'state' and - * 'state + 4'. - * - * Because we know the expected values of each - * received bit are flipped between these two previous - * states: - * - * bitcost(state+4) = 510 - bitcost(state) - * - * With those two total costs in hand, we then pick - * the lower as the cost of the 'state<<1', and compute - * the path of bits leading to that state. - * - * Then, do the same for '(state<<1) + 1'. This time, - * instead of computing the m0 and m1 values from - * scratch, because the only difference is that we're - * expecting a one bit instead of a zero bit, we just - * flip the bitcost values around to match the - * expected transmitted bits with some tricky - * arithmetic which is equivalent to: - * - * m0 = cost[p][state] + (510 - bitcost); - * m1 = cost[p][state+4] + bitcost - * - * Then, the lowest cost and bit trace of the new state - * is saved. - */ - -#define DO_STATE(state) { \ - uint32_t bitcost; \ - \ - uint32_t m0; \ - uint32_t m1; \ - uint32_t bit; \ - \ - bitcost = ((uint32_t) (s0 ^ ao_fec_decode_table[(state<<1)]) + \ - (uint32_t) (s1 ^ ao_fec_decode_table[(state<<1)|1])); \ - \ - m0 = cost[p][state] + bitcost; \ - m1 = cost[p][state+4] + (510 - bitcost); \ - bit = m0 > m1; \ - cost[n][state<<1] = bit ? m1 : m0; \ - bits[n][state<<1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \ - \ - m0 -= (bitcost+bitcost-510); \ - m1 += (bitcost+bitcost-510); \ - bit = m0 > m1; \ - cost[n][(state<<1)+1] = bit ? m1 : m0; \ - bits[n][(state<<1)+1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \ - } - - DO_STATE(0); - DO_STATE(1); - DO_STATE(2); - DO_STATE(3); - -#if 0 - printf ("bit %3d symbol %2x %2x:", i/2, s0, s1); - for (state = 0; state < NUM_STATE; state++) { - printf (" %8u(%08x)", cost[n][state], bits[n][state]); - } - printf ("\n"); -#endif - p = n; - - /* A loop is needed to handle the last output byte. It - * won't have any bits of future data to perform full - * error correction, but we might as well give the - * best possible answer anyways. - */ - while ((b - o) >= (8 + NUM_HIST) || (i + 2 >= len && b > o)) { - - /* Compute number of bits to the end of the - * last full byte of data. This is generally - * NUM_HIST, unless we've reached - * the end of the input, in which case - * it will be seven. - */ - int8_t dist = b - (o + 8); /* distance to last ready-for-writing bit */ - uint32_t min_cost; /* lowest cost */ - uint8_t min_state; /* lowest cost state */ - uint8_t byte; - - /* Find the best fit at the current point - * of the decode. - */ - min_cost = cost[p][0]; - min_state = 0; - for (state = 1; state < NUM_STATE; state++) { - if (cost[p][state] < min_cost) { - min_cost = cost[p][state]; - min_state = state; - } - } - - /* The very last byte of data has the very last bit - * of data left in the state value; just smash the - * bits value in place and reset the 'dist' from - * -1 to 0 so that the full byte is read out - */ - if (dist < 0) { - bits[p][min_state] = (bits[p][min_state] << 1) | (min_state & 1); - dist = 0; - } - -#if 0 - printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n", - i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten); -#endif - byte = (bits[p][min_state] >> dist) ^ *whiten++; - *out++ = byte; - if (out_len > 2) - crc = ao_fec_crc_byte(byte, crc); - - if (!--out_len) { - if ((out[-2] == (uint8_t) (crc >> 8)) && - out[-1] == (uint8_t) crc) - out[-1] = AO_FEC_DECODE_CRC_OK; - else - out[-1] = 0; - out[-2] = 0; - goto done; - } - o += 8; - } - } -done: -#if AO_PROFILE - ao_fec_decode_start = start_tick; - ao_fec_decode_end = ao_profile_tick(); -#endif - return 1; -} diff --git a/src/core/ao_fec_tx.c b/src/core/ao_fec_tx.c deleted file mode 100644 index 4941d745..00000000 --- a/src/core/ao_fec_tx.c +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include - -#if AO_FEC_DEBUG -void -ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name) -{ - uint16_t i; - - printf ("%s (%d):", name, len); - for (i = 0; i < len; i++) { - if ((i & 7) == 0) - printf ("\n\t%02x:", i); - printf(" %02x", bytes[i]); - } - printf ("\n"); -} -#endif - -uint16_t -ao_fec_crc(const uint8_t *bytes, uint8_t len) -{ - uint16_t crc = AO_FEC_CRC_INIT; - - while (len--) - crc = ao_fec_crc_byte(*bytes++, crc); - return crc; -} - -/* - * len is the length of the data; the crc will be - * the fist two bytes after that - */ - -uint8_t -ao_fec_check_crc(const uint8_t *bytes, uint8_t len) -{ - uint16_t computed_crc = ao_fec_crc(bytes, len); - uint16_t received_crc = (bytes[len] << 8) | (bytes[len+1]); - - return computed_crc == received_crc; -} - -/* - * Compute CRC and trellis-terminator/interleave-pad bytes - */ -static uint8_t -ao_fec_prepare(const uint8_t *in, uint8_t len, uint8_t *extra) -{ - uint16_t crc = ao_fec_crc (in, len); - uint8_t i = 0; - uint8_t num_fec; - - /* Append CRC */ - extra[i++] = crc >> 8; - extra[i++] = crc; - - /* Append FEC -- 1 byte if odd, two bytes if even */ - num_fec = 2 - (i & 1); - while (num_fec--) - extra[i++] = AO_FEC_TRELLIS_TERMINATOR; - return i; -} - -const uint8_t ao_fec_whiten_table[] = { -#include "ao_whiten.h" -}; - -static const uint8_t ao_fec_encode_table[16] = { -/* next 0 1 state */ - 0, 3, /* 000 */ - 1, 2, /* 001 */ - 3, 0, /* 010 */ - 2, 1, /* 011 */ - 3, 0, /* 100 */ - 2, 1, /* 101 */ - 0, 3, /* 110 */ - 1, 2 /* 111 */ -}; - -uint8_t -ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out) -{ - uint8_t extra[AO_FEC_PREPARE_EXTRA]; - uint8_t extra_len; - uint32_t encode, interleave; - uint8_t pair, byte, bit; - uint16_t fec = 0; - const uint8_t *whiten = ao_fec_whiten_table; - - extra_len = ao_fec_prepare(in, len, extra); - for (pair = 0; pair < len + extra_len; pair += 2) { - encode = 0; - for (byte = 0; byte < 2; byte++) { - if (pair + byte == len) - in = extra; - fec |= *in++ ^ *whiten++; - for (bit = 0; bit < 8; bit++) { - encode = encode << 2 | ao_fec_encode_table[fec >> 7]; - fec = (fec << 1) & 0x7ff; - } - } - - interleave = 0; - for (bit = 0; bit < 4 * 4; bit++) { - uint8_t byte_shift = (bit & 0x3) << 3; - uint8_t bit_shift = (bit & 0xc) >> 1; - - interleave = (interleave << 2) | ((encode >> (byte_shift + bit_shift)) & 0x3); - } - *out++ = interleave >> 24; - *out++ = interleave >> 16; - *out++ = interleave >> 8; - *out++ = interleave >> 0; - } - return (len + extra_len) * 2; -} diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c deleted file mode 100644 index 24099347..00000000 --- a/src/core/ao_flight.c +++ /dev/null @@ -1,472 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_FLIGHT_TEST -#include "ao.h" -#include -#endif - -#if HAS_MPU6000 -#include -#endif - -#ifndef HAS_ACCEL -#error Please define HAS_ACCEL -#endif - -#ifndef HAS_GPS -#error Please define HAS_GPS -#endif - -#ifndef HAS_USB -#error Please define HAS_USB -#endif - -#ifndef HAS_TELEMETRY -#define HAS_TELEMETRY HAS_RADIO -#endif - -/* Main flight thread. */ - -__pdata enum ao_flight_state ao_flight_state; /* current flight state */ -__pdata uint16_t ao_boost_tick; /* time of launch detect */ -__pdata uint16_t ao_motor_number; /* number of motors burned so far */ - -#if HAS_SENSOR_ERRORS -/* Any sensor can set this to mark the flight computer as 'broken' */ -__xdata uint8_t ao_sensor_errors; -#endif - -/* - * track min/max data over a long interval to detect - * resting - */ -static __data uint16_t ao_interval_end; -static __data int16_t ao_interval_min_height; -static __data int16_t ao_interval_max_height; -#if HAS_ACCEL -static __data int16_t ao_coast_avg_accel; -#endif - -__pdata uint8_t ao_flight_force_idle; - -/* We also have a clock, which can be used to sanity check things in - * case of other failures - */ - -#define BOOST_TICKS_MAX AO_SEC_TO_TICKS(15) - -/* Landing is detected by getting constant readings from both pressure and accelerometer - * for a fairly long time (AO_INTERVAL_TICKS) - */ -#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(10) - -#define abs(a) ((a) < 0 ? -(a) : (a)) - -void -ao_flight(void) -{ - ao_sample_init(); - ao_flight_state = ao_flight_startup; - for (;;) { - - /* - * Process ADC samples, just looping - * until the sensors are calibrated. - */ - if (!ao_sample()) - continue; - - switch (ao_flight_state) { - case ao_flight_startup: - - /* Check to see what mode we should go to. - * - Invalid mode if accel cal appears to be out - * - pad mode if we're upright, - * - idle mode otherwise - */ -#if HAS_ACCEL - if (ao_config.accel_plus_g == 0 || - ao_config.accel_minus_g == 0 || - ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP || - ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP || - ao_ground_height < -1000 || - ao_ground_height > 7000) - { - /* Detected an accel value outside -1.5g to 1.5g - * (or uncalibrated values), so we go into invalid mode - */ - ao_flight_state = ao_flight_invalid; - -#if HAS_RADIO && PACKET_HAS_SLAVE - /* Turn on packet system in invalid mode on TeleMetrum */ - ao_packet_slave_start(); -#endif - } else -#endif - if (!ao_flight_force_idle -#if HAS_ACCEL - && ao_ground_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP -#endif - ) - { - /* Set pad mode - we can fly! */ - ao_flight_state = ao_flight_pad; -#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE - /* Disable the USB controller in flight mode - * to save power - */ - ao_usb_disable(); -#endif - -#if !HAS_ACCEL && PACKET_HAS_SLAVE - /* Disable packet mode in pad state on TeleMini */ - ao_packet_slave_stop(); -#endif - -#if HAS_TELEMETRY - /* Turn on telemetry system */ - ao_rdf_set(1); - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); -#endif -#if AO_LED_RED - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); -#endif - } else { - /* Set idle mode */ - ao_flight_state = ao_flight_idle; -#if HAS_SENSOR_ERRORS - if (ao_sensor_errors) - ao_flight_state = ao_flight_invalid; -#endif - -#if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE - /* Turn on packet system in idle mode on TeleMetrum */ - ao_packet_slave_start(); -#endif - -#if AO_LED_RED - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); -#endif - } - /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - - break; - case ao_flight_pad: - - /* pad to boost: - * - * barometer: > 20m vertical motion - * OR - * accelerometer: > 2g AND velocity > 5m/s - * - * The accelerometer should always detect motion before - * the barometer, but we use both to make sure this - * transition is detected. If the device - * doesn't have an accelerometer, then ignore the - * speed and acceleration as they are quite noisy - * on the pad. - */ - if (ao_height > AO_M_TO_HEIGHT(20) -#if HAS_ACCEL - || (ao_accel > AO_MSS_TO_ACCEL(20) && - ao_speed > AO_MS_TO_SPEED(5)) -#endif - ) - { - ao_flight_state = ao_flight_boost; - ao_boost_tick = ao_sample_tick; - - /* start logging data */ - ao_log_start(); - -#if HAS_TELEMETRY - /* Increase telemetry rate */ - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT); - - /* disable RDF beacon */ - ao_rdf_set(0); -#endif - -#if HAS_GPS - /* Record current GPS position by waking up GPS log tasks */ - ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; - ao_wakeup(&ao_gps_new); -#endif - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; - case ao_flight_boost: - - /* boost to fast: - * - * accelerometer: start to fall at > 1/4 G - * OR - * time: boost for more than 15 seconds - * - * Detects motor burn out by the switch from acceleration to - * deceleration, or by waiting until the maximum burn duration - * (15 seconds) has past. - */ - if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) || - (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) - { -#if HAS_ACCEL - ao_flight_state = ao_flight_fast; - ao_coast_avg_accel = ao_accel; -#else - ao_flight_state = ao_flight_coast; -#endif - ++ao_motor_number; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; -#if HAS_ACCEL - case ao_flight_fast: - /* - * This is essentially the same as coast, - * but the barometer is being ignored as - * it may be unreliable. - */ - if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) - { - ao_flight_state = ao_flight_coast; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } else - goto check_re_boost; - break; -#endif - case ao_flight_coast: - - /* - * By customer request - allow the user - * to lock out apogee detection for a specified - * number of seconds. - */ - if (ao_config.apogee_lockout) { - if ((ao_sample_tick - ao_boost_tick) < - AO_SEC_TO_TICKS(ao_config.apogee_lockout)) - break; - } - - /* apogee detect: coast to drogue deploy: - * - * speed: < 0 - * - * Also make sure the model altitude is tracking - * the measured altitude reasonably closely; otherwise - * we're probably transsonic. - */ - if (ao_speed < 0 -#if !HAS_ACCEL - && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100) -#endif - ) - { -#if HAS_IGNITE - /* ignite the drogue charge */ - ao_ignite(ao_igniter_drogue); -#endif - -#if HAS_TELEMETRY - /* slow down the telemetry system */ - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER); - - /* Turn the RDF beacon back on */ - ao_rdf_set(1); -#endif - - /* and enter drogue state */ - ao_flight_state = ao_flight_drogue; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } -#if HAS_ACCEL - else { - check_re_boost: - ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6); - if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) { - ao_boost_tick = ao_sample_tick; - ao_flight_state = ao_flight_boost; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - } -#endif - - break; - case ao_flight_drogue: - - /* drogue to main deploy: - * - * barometer: reach main deploy altitude - * - * Would like to use the accelerometer for this test, but - * the orientation of the flight computer is unknown after - * drogue deploy, so we ignore it. Could also detect - * high descent rate using the pressure sensor to - * recognize drogue deploy failure and eject the main - * at that point. Perhaps also use the drogue sense lines - * to notice continutity? - */ - if (ao_height <= ao_config.main_deploy) - { -#if HAS_IGNITE - ao_ignite(ao_igniter_main); -#endif - - /* - * Start recording min/max height - * to figure out when the rocket has landed - */ - - /* initialize interval values */ - ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - - ao_interval_min_height = ao_interval_max_height = ao_avg_height; - - ao_flight_state = ao_flight_main; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; - - /* fall through... */ - case ao_flight_main: - - /* main to land: - * - * barometer: altitude stable - */ - - if (ao_avg_height < ao_interval_min_height) - ao_interval_min_height = ao_avg_height; - if (ao_avg_height > ao_interval_max_height) - ao_interval_max_height = ao_avg_height; - - if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { - if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4)) - { - ao_flight_state = ao_flight_landed; - - /* turn off the ADC capture */ - ao_timer_set_adc_interval(0); - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - ao_interval_min_height = ao_interval_max_height = ao_avg_height; - ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - } - break; -#if HAS_FLIGHT_DEBUG - case ao_flight_test: -#if HAS_GYRO - printf ("angle %4d pitch %7d yaw %7d roll %7d\n", - ao_sample_orient, - ((ao_sample_pitch << 9) - ao_ground_pitch) >> 9, - ((ao_sample_yaw << 9) - ao_ground_yaw) >> 9, - ((ao_sample_roll << 9) - ao_ground_roll) >> 9); -#endif - flush(); - break; -#endif /* HAS_FLIGHT_DEBUG */ - default: - break; - } - } -} - -#if HAS_FLIGHT_DEBUG -static inline int int_part(int16_t i) { return i >> 4; } -static inline int frac_part(int16_t i) { return ((i & 0xf) * 100 + 8) / 16; } - -static void -ao_flight_dump(void) -{ -#if HAS_ACCEL - int16_t accel; - - accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; -#endif - - printf ("sample:\n"); - printf (" tick %d\n", ao_sample_tick); - printf (" raw pres %d\n", ao_sample_pres); -#if HAS_ACCEL - printf (" raw accel %d\n", ao_sample_accel); -#endif - printf (" ground pres %d\n", ao_ground_pres); - printf (" ground alt %d\n", ao_ground_height); -#if HAS_ACCEL - printf (" raw accel %d\n", ao_sample_accel); - printf (" groundaccel %d\n", ao_ground_accel); - printf (" accel_2g %d\n", ao_accel_2g); -#endif - - printf (" alt %d\n", ao_sample_alt); - printf (" height %d\n", ao_sample_height); -#if HAS_ACCEL - printf (" accel %d.%02d\n", int_part(accel), frac_part(accel)); -#endif - - - printf ("kalman:\n"); - printf (" height %d\n", ao_height); - printf (" speed %d.%02d\n", int_part(ao_speed), frac_part(ao_speed)); - printf (" accel %d.%02d\n", int_part(ao_accel), frac_part(ao_accel)); - printf (" max_height %d\n", ao_max_height); - printf (" avg_height %d\n", ao_avg_height); - printf (" error_h %d\n", ao_error_h); - printf (" error_avg %d\n", ao_error_h_sq_avg); -} - -static void -ao_gyro_test(void) -{ - ao_flight_state = ao_flight_test; - ao_getchar(); - ao_flight_state = ao_flight_idle; -} - -uint8_t ao_orient_test; - -static void -ao_orient_test_select(void) -{ - ao_orient_test = !ao_orient_test; -} - -__code struct ao_cmds ao_flight_cmds[] = { - { ao_flight_dump, "F\0Dump flight status" }, - { ao_gyro_test, "G\0Test gyro code" }, - { ao_orient_test_select,"O\0Test orientation code" }, - { 0, NULL }, -}; -#endif - -static __xdata struct ao_task flight_task; - -void -ao_flight_init(void) -{ - ao_flight_state = ao_flight_startup; -#if HAS_FLIGHT_DEBUG - ao_cmd_register(&ao_flight_cmds[0]); -#endif - ao_add_task(&flight_task, ao_flight, "flight"); -} diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h deleted file mode 100644 index 01d21c11..00000000 --- a/src/core/ao_flight.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_FLIGHT_H_ -#define _AO_FLIGHT_H_ - - -/* - * ao_flight.c - */ - -enum ao_flight_state { - ao_flight_startup = 0, - ao_flight_idle = 1, - ao_flight_pad = 2, - ao_flight_boost = 3, - ao_flight_fast = 4, - ao_flight_coast = 5, - ao_flight_drogue = 6, - ao_flight_main = 7, - ao_flight_landed = 8, - ao_flight_invalid = 9, - ao_flight_test = 10 -}; - -extern __pdata enum ao_flight_state ao_flight_state; -extern __pdata uint16_t ao_boost_tick; -extern __pdata uint16_t ao_motor_number; - -#if HAS_IMU || HAS_MMA655X -#define HAS_SENSOR_ERRORS 1 -#endif - -#if HAS_SENSOR_ERRORS -extern __xdata uint8_t ao_sensor_errors; -#endif - -extern __pdata uint16_t ao_launch_time; -extern __pdata uint8_t ao_flight_force_idle; - -/* Flight thread */ -void -ao_flight(void); - -/* Initialize flight thread */ -void -ao_flight_init(void); - -/* - * ao_flight_nano.c - */ - -void -ao_flight_nano_init(void); - -#endif /* _AO_FLIGHT_H_ */ diff --git a/src/core/ao_flight_nano.c b/src/core/ao_flight_nano.c deleted file mode 100644 index 406d81ad..00000000 --- a/src/core/ao_flight_nano.c +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -/* Main flight thread. */ - -__pdata enum ao_flight_state ao_flight_state; /* current flight state */ -__pdata uint16_t ao_launch_tick; /* time of launch detect */ - -/* - * track min/max data over a long interval to detect - * resting - */ -__pdata uint16_t ao_interval_end; -__pdata alt_t ao_interval_min_height; -__pdata alt_t ao_interval_max_height; - -__pdata uint8_t ao_flight_force_idle; - -/* Landing is detected by getting constant readings from both pressure and accelerometer - * for a fairly long time (AO_INTERVAL_TICKS) - */ -#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(5) - -static void -ao_flight_nano(void) -{ - ao_sample_init(); - ao_flight_state = ao_flight_startup; - - for (;;) { - /* - * Process ADC samples, just looping - * until the sensors are calibrated. - */ - if (!ao_sample()) - continue; - - switch (ao_flight_state) { - case ao_flight_startup: - if (ao_flight_force_idle) { - /* Set idle mode */ - ao_flight_state = ao_flight_idle; - } else { - ao_flight_state = ao_flight_pad; - /* Disable packet mode in pad state */ - ao_packet_slave_stop(); - - /* Turn on telemetry system */ - ao_rdf_set(1); - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); - } - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); - - /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - break; - case ao_flight_pad: - if (ao_height> AO_M_TO_HEIGHT(20)) { - ao_flight_state = ao_flight_drogue; - ao_launch_tick = ao_sample_tick; - - /* start logging data */ - ao_log_start(); - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; - case ao_flight_drogue: - /* drogue/main to land: - * - * barometer: altitude stable - */ - - if (ao_height < ao_interval_min_height) - ao_interval_min_height = ao_height; - if (ao_height > ao_interval_max_height) - ao_interval_max_height = ao_height; - - if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { - if (ao_interval_max_height - ao_interval_min_height < AO_M_TO_HEIGHT(5)) - { - ao_flight_state = ao_flight_landed; - - /* turn off the ADC capture */ - ao_timer_set_adc_interval(0); - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - ao_interval_min_height = ao_interval_max_height = ao_height; - ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - } - break; - } - } -} - -static __xdata struct ao_task flight_task; - -void -ao_flight_nano_init(void) -{ - ao_flight_state = ao_flight_startup; - ao_add_task(&flight_task, ao_flight_nano, "flight"); -} diff --git a/src/core/ao_freq.c b/src/core/ao_freq.c deleted file mode 100644 index 12496f6f..00000000 --- a/src/core/ao_freq.c +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include - -/* - * The provided 'calibration' value is - * that needed to tune the radio to precisely 434550kHz. - * Use that to 'walk' to the target frequency by following - * a 'bresenham' line from 434550kHz to the target - * frequency, and updating the radio setting along the way - */ - -int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant -{ - static __pdata int32_t set; - static __pdata uint8_t neg; - static __pdata int32_t error; - - set = 0; - neg = 0; - error = -434550 / 2; - - if ((freq -= 434550) < 0) { - neg = 1; - freq = -freq; - } - for (;;) { - if (error > 0) { - error -= 434550; - set++; - } else { - error += cal; - if (--freq < 0) - break; - } - } - if (neg) - set = -set; - return cal + set; -} diff --git a/src/core/ao_gps_print.c b/src/core/ao_gps_print.c deleted file mode 100644 index 47c945d7..00000000 --- a/src/core/ao_gps_print.c +++ /dev/null @@ -1,112 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_GPS_TEST -#include "ao.h" -#endif -#include "ao_telem.h" - -void -ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant -{ - char state; - - if (gps_data->flags & AO_GPS_VALID) - state = AO_TELEM_GPS_STATE_LOCKED; - else if (gps_data->flags & AO_GPS_RUNNING) - state = AO_TELEM_GPS_STATE_UNLOCKED; - else - state = AO_TELEM_GPS_STATE_ERROR; - printf(AO_TELEM_GPS_STATE " %c " - AO_TELEM_GPS_NUM_SAT " %d ", - state, - (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT); - if (!(gps_data->flags & AO_GPS_VALID)) - return; - printf(AO_TELEM_GPS_LATITUDE " %ld " - AO_TELEM_GPS_LONGITUDE " %ld " - AO_TELEM_GPS_ALTITUDE " %d ", - (long) gps_data->latitude, - (long) gps_data->longitude, - gps_data->altitude); - - if (gps_data->flags & AO_GPS_DATE_VALID) - printf(AO_TELEM_GPS_YEAR " %d " - AO_TELEM_GPS_MONTH " %d " - AO_TELEM_GPS_DAY " %d ", - gps_data->year, - gps_data->month, - gps_data->day); - - printf(AO_TELEM_GPS_HOUR " %d " - AO_TELEM_GPS_MINUTE " %d " - AO_TELEM_GPS_SECOND " %d ", - gps_data->hour, - gps_data->minute, - gps_data->second); - - printf(AO_TELEM_GPS_HDOP " %d ", - gps_data->hdop * 2); - - if (gps_data->flags & AO_GPS_COURSE_VALID) { - printf(AO_TELEM_GPS_HERROR " %d " - AO_TELEM_GPS_VERROR " %d " - AO_TELEM_GPS_VERTICAL_SPEED " %d " - AO_TELEM_GPS_HORIZONTAL_SPEED " %d " - AO_TELEM_GPS_COURSE " %d ", - gps_data->h_error, - gps_data->v_error, - gps_data->climb_rate, - gps_data->ground_speed, - (int) gps_data->course * 2); - } -} - -void -ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data) __reentrant -{ - uint8_t c, n, v; - __xdata struct ao_gps_sat_orig *sat; - - n = gps_tracking_data->channels; - if (n == 0) - return; - - sat = gps_tracking_data->sats; - v = 0; - for (c = 0; c < n; c++) { - if (sat->svid) - v++; - sat++; - } - - printf (AO_TELEM_SAT_NUM " %d ", - v); - - sat = gps_tracking_data->sats; - v = 0; - for (c = 0; c < n; c++) { - if (sat->svid) { - printf (AO_TELEM_SAT_SVID "%d %d " - AO_TELEM_SAT_C_N_0 "%d %d ", - v, sat->svid, - v, sat->c_n_1); - v++; - } - sat++; - } -} diff --git a/src/core/ao_gps_report.c b/src/core/ao_gps_report.c deleted file mode 100644 index 07201ac2..00000000 --- a/src/core/ao_gps_report.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -void -ao_gps_report(void) -{ - static __xdata struct ao_log_record gps_log; - static __xdata struct ao_telemetry_location gps_data; - static __xdata struct ao_telemetry_satellite gps_tracking_data; - uint8_t date_reported = 0; - uint8_t new; - - for (;;) { - while ((new = ao_gps_new) == 0) - ao_sleep(&ao_gps_new); - ao_mutex_get(&ao_gps_mutex); - if (new & AO_GPS_NEW_DATA) - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); - if (new & AO_GPS_NEW_TRACKING) - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); - ao_gps_new = 0; - ao_mutex_put(&ao_gps_mutex); - - if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_TIME; - gps_log.u.gps_time.hour = gps_data.hour; - gps_log.u.gps_time.minute = gps_data.minute; - gps_log.u.gps_time.second = gps_data.second; - gps_log.u.gps_time.flags = gps_data.flags; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_LAT; - gps_log.u.gps_latitude = gps_data.latitude; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_LON; - gps_log.u.gps_longitude = gps_data.longitude; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_ALT; - gps_log.u.gps_altitude.altitude = gps_data.altitude; - gps_log.u.gps_altitude.unused = 0xffff; - ao_log_data(&gps_log); - if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) { - gps_log.type = AO_LOG_GPS_DATE; - gps_log.u.gps_date.year = gps_data.year; - gps_log.u.gps_date.month = gps_data.month; - gps_log.u.gps_date.day = gps_data.day; - gps_log.u.gps_date.extra = 0; - date_reported = ao_log_data(&gps_log); - } - } - if (new & AO_GPS_NEW_TRACKING) { - uint8_t c, n; - - if ((n = gps_tracking_data.channels) != 0) { - gps_log.type = AO_LOG_GPS_SAT; - for (c = 0; c < n; c++) - if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid)) - { - gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1; - ao_log_data(&gps_log); - } - } - } - } -} - -__xdata struct ao_task ao_gps_report_task; - -void -ao_gps_report_init(void) -{ - ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report"); -} diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c deleted file mode 100644 index 5e3c71bf..00000000 --- a/src/core/ao_gps_report_mega.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include "ao_log.h" - -void -ao_gps_report_mega(void) -{ - static __xdata struct ao_log_mega gps_log; - static __xdata struct ao_telemetry_location gps_data; - static __xdata struct ao_telemetry_satellite gps_tracking_data; - uint8_t new; - uint8_t c, n, i; - - for (;;) { - while (!(new = ao_gps_new)) - ao_sleep(&ao_gps_new); - ao_mutex_get(&ao_gps_mutex); - if (new & AO_GPS_NEW_DATA) - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); - if (new & AO_GPS_NEW_TRACKING) - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); - ao_gps_new = 0; - ao_mutex_put(&ao_gps_mutex); - - if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_TIME; - gps_log.u.gps.latitude = gps_data.latitude; - gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; - - gps_log.u.gps.hour = gps_data.hour; - gps_log.u.gps.minute = gps_data.minute; - gps_log.u.gps.second = gps_data.second; - gps_log.u.gps.flags = gps_data.flags; - gps_log.u.gps.year = gps_data.year; - gps_log.u.gps.month = gps_data.month; - gps_log.u.gps.day = gps_data.day; - gps_log.u.gps.course = gps_data.course; - gps_log.u.gps.ground_speed = gps_data.ground_speed; - gps_log.u.gps.climb_rate = gps_data.climb_rate; - gps_log.u.gps.pdop = gps_data.pdop; - gps_log.u.gps.hdop = gps_data.hdop; - gps_log.u.gps.vdop = gps_data.vdop; - gps_log.u.gps.mode = gps_data.mode; - ao_log_mega(&gps_log); - } - if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels) != 0) { - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_SAT; - i = 0; - for (c = 0; c < n; c++) - if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid)) - { - gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; - i++; - if (i >= 12) - break; - } - gps_log.u.gps_sat.channels = i; - ao_log_mega(&gps_log); - } - } -} - -__xdata struct ao_task ao_gps_report_mega_task; - -void -ao_gps_report_mega_init(void) -{ - ao_add_task(&ao_gps_report_mega_task, - ao_gps_report_mega, - "gps_report"); -} diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c deleted file mode 100644 index 696a833b..00000000 --- a/src/core/ao_gps_report_metrum.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include "ao_log.h" - -void -ao_gps_report_metrum(void) -{ - static __xdata struct ao_log_metrum gps_log; - static __xdata struct ao_telemetry_location gps_data; - static __xdata struct ao_telemetry_satellite gps_tracking_data; - uint8_t c, n, i; - uint8_t svid; - uint8_t new; - - for (;;) { - while (!(new = ao_gps_new)) - ao_sleep(&ao_gps_new); - ao_mutex_get(&ao_gps_mutex); - if (new & AO_GPS_NEW_DATA) - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); - if (new & AO_GPS_NEW_TRACKING) - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); - ao_gps_new = 0; - ao_mutex_put(&ao_gps_mutex); - - if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_POS; - gps_log.u.gps.latitude = gps_data.latitude; - gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; - ao_log_metrum(&gps_log); - - gps_log.type = AO_LOG_GPS_TIME; - gps_log.u.gps_time.hour = gps_data.hour; - gps_log.u.gps_time.minute = gps_data.minute; - gps_log.u.gps_time.second = gps_data.second; - gps_log.u.gps_time.flags = gps_data.flags; - gps_log.u.gps_time.year = gps_data.year; - gps_log.u.gps_time.month = gps_data.month; - gps_log.u.gps_time.day = gps_data.day; - ao_log_metrum(&gps_log); - } - - if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels)) { - gps_log.type = AO_LOG_GPS_SAT; - gps_log.tick = ao_gps_tick; - i = 0; - for (c = 0; c < n; c++) { - svid = gps_tracking_data.sats[c].svid; - if (svid != 0) { - if (i == 4) { - gps_log.u.gps_sat.channels = i; - gps_log.u.gps_sat.more = 1; - ao_log_metrum(&gps_log); - i = 0; - } - gps_log.u.gps_sat.sats[i].svid = svid; - gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; - i++; - } - } - if (i) { - gps_log.u.gps_sat.channels = i; - gps_log.u.gps_sat.more = 0; - ao_log_metrum(&gps_log); - } - } - } -} - -__xdata struct ao_task ao_gps_report_metrum_task; - -void -ao_gps_report_metrum_init(void) -{ - ao_add_task(&ao_gps_report_metrum_task, - ao_gps_report_metrum, - "gps_report"); -} diff --git a/src/core/ao_gps_show.c b/src/core/ao_gps_show.c deleted file mode 100644 index 3a05e35a..00000000 --- a/src/core/ao_gps_show.c +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_GPS_TEST -#include -#endif - -void -ao_gps_show(void) __reentrant -{ - uint8_t i; - ao_mutex_get(&ao_gps_mutex); - printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day); - printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); - printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude); - printf ("Alt: %d\n", ao_gps_data.altitude); - printf ("Flags: 0x%x\n", ao_gps_data.flags); - printf ("Sats: %d", ao_gps_tracking_data.channels); - for (i = 0; i < ao_gps_tracking_data.channels; i++) - printf (" %d %d", - ao_gps_tracking_data.sats[i].svid, - ao_gps_tracking_data.sats[i].c_n_1); - printf ("\ndone\n"); - ao_mutex_put(&ao_gps_mutex); -} diff --git a/src/core/ao_host.h b/src/core/ao_host.h deleted file mode 100644 index 6eb752c9..00000000 --- a/src/core/ao_host.h +++ /dev/null @@ -1,135 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#define _GNU_SOURCE - -#include -#include -#include -#include -#include - -#define AO_ADC_RING 64 -#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) -#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) - -/* - * One set of samples read from the A/D converter - */ -struct ao_adc { - uint16_t tick; /* tick when the sample was read */ - int16_t accel; /* accelerometer */ - int16_t pres; /* pressure sensor */ - int16_t temp; /* temperature sensor */ - int16_t v_batt; /* battery voltage */ - int16_t sense_d; /* drogue continuity sense */ - int16_t sense_m; /* main continuity sense */ -}; - -#define __pdata -#define __data -#define __xdata -#define __code -#define __reentrant - -#define DATA_TO_XDATA(a) (a) -#define PDATA_TO_XDATA(a) (a) -#define CODE_TO_XDATA(a) (a) - -enum ao_flight_state { - ao_flight_startup = 0, - ao_flight_idle = 1, - ao_flight_pad = 2, - ao_flight_boost = 3, - ao_flight_fast = 4, - ao_flight_coast = 5, - ao_flight_drogue = 6, - ao_flight_main = 7, - ao_flight_landed = 8, - ao_flight_invalid = 9 -}; - -struct ao_adc ao_adc_ring[AO_ADC_RING]; -uint8_t ao_adc_head; - -#define ao_led_on(l) -#define ao_led_off(l) -#define ao_timer_set_adc_interval(i) -#define ao_wakeup(wchan) ao_dump_state(wchan) -#define ao_cmd_register(c) -#define ao_usb_disable() -#define ao_telemetry_set_interval(x) -#define ao_delay(x) - -enum ao_igniter { - ao_igniter_drogue = 0, - ao_igniter_main = 1 -}; - -void -ao_ignite(enum ao_igniter igniter) -{ - printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); -} - -struct ao_task { - int dummy; -}; - -#define ao_add_task(t,f,n) - -#define ao_log_start() -#define ao_log_stop() - -#define AO_MS_TO_TICKS(ms) ((ms) / 10) -#define AO_SEC_TO_TICKS(s) ((s) * 100) - -#define AO_FLIGHT_TEST - -struct ao_adc ao_adc_static; - -FILE *emulator_in; - -void -ao_dump_state(void *wchan); - -void -ao_sleep(void *wchan); - -const char const * const ao_state_names[] = { - "startup", "idle", "pad", "boost", "fast", - "coast", "drogue", "main", "landed", "invalid" -}; - -struct ao_cmds { - void (*func)(void); - const char *help; -}; - - -struct ao_config { - uint16_t main_deploy; - int16_t accel_zero_g; -}; - -#define ao_config_get() - -struct ao_config ao_config = { 250, 16000 }; - -#define ao_xmemcpy(d,s,c) memcpy(d,s,c) -#define ao_xmemset(d,v,c) memset(d,v,c) -#define ao_xmemcmp(d,s,c) memcmp(d,s,c) diff --git a/src/core/ao_ignite.c b/src/core/ao_ignite.c deleted file mode 100644 index 823d003c..00000000 --- a/src/core/ao_ignite.c +++ /dev/null @@ -1,241 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include -#if AO_PYRO_NUM -#include -#endif - -#if HAS_IGNITE -__xdata struct ao_ignition ao_ignition[2]; - -void -ao_ignite(enum ao_igniter igniter) -{ - ao_arch_block_interrupts(); - ao_ignition[igniter].request = 1; - ao_wakeup(&ao_ignition); - ao_arch_release_interrupts(); -} - -#ifndef AO_SENSE_DROGUE -#define AO_SENSE_DROGUE(p) ((p)->adc.sense_d) -#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) -#endif - -enum ao_igniter_status -ao_igniter_status(enum ao_igniter igniter) -{ - __xdata struct ao_data packet; - __pdata int16_t value; - __pdata uint8_t request, firing, fired; - - ao_arch_critical( - ao_data_get(&packet); - request = ao_ignition[igniter].request; - fired = ao_ignition[igniter].fired; - firing = ao_ignition[igniter].firing; - ); - if (firing || (request && !fired)) - return ao_igniter_active; - - value = (AO_IGNITER_CLOSED>>1); - switch (igniter) { - case ao_igniter_drogue: - value = AO_SENSE_DROGUE(&packet); - break; - case ao_igniter_main: - value = AO_SENSE_MAIN(&packet); - break; - } - if (value < AO_IGNITER_OPEN) - return ao_igniter_open; - else if (value > AO_IGNITER_CLOSED) - return ao_igniter_ready; - else - return ao_igniter_unknown; -} - -#ifndef AO_IGNITER_SET_DROGUE -#define AO_IGNITER_SET_DROGUE(v) AO_IGNITER_DROGUE = (v) -#define AO_IGNITER_SET_MAIN(v) AO_IGNITER_MAIN = (v) -#endif - -#ifndef AO_IGNITER_FIRE_TIME -#define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) -#endif - -#ifndef AO_IGNITER_CHARGE_TIME -#define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) -#endif - -void -ao_igniter_fire(enum ao_igniter igniter) -{ - ao_ignition[igniter].firing = 1; - switch(ao_config.ignite_mode) { - case AO_IGNITE_MODE_DUAL: - switch (igniter) { - case ao_igniter_drogue: - AO_IGNITER_SET_DROGUE(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_DROGUE(0); - break; - case ao_igniter_main: - AO_IGNITER_SET_MAIN(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_MAIN(0); - break; - } - break; - case AO_IGNITE_MODE_APOGEE: - switch (igniter) { - case ao_igniter_drogue: - AO_IGNITER_SET_DROGUE(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_DROGUE(0); - ao_delay(AO_IGNITER_CHARGE_TIME); - AO_IGNITER_SET_MAIN(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_MAIN(0); - break; - default: - break; - } - break; - case AO_IGNITE_MODE_MAIN: - switch (igniter) { - case ao_igniter_main: - AO_IGNITER_SET_DROGUE(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_DROGUE(0); - ao_delay(AO_IGNITER_CHARGE_TIME); - AO_IGNITER_SET_MAIN(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_MAIN(0); - break; - default: - break; - } - break; - } - ao_ignition[igniter].firing = 0; -} - -void -ao_igniter(void) -{ - __xdata enum ao_igniter igniter; - - ao_config_get(); - for (;;) { - ao_sleep(&ao_ignition); - for (igniter = ao_igniter_drogue; igniter <= ao_igniter_main; igniter++) { - if (ao_ignition[igniter].request && !ao_ignition[igniter].fired) { - if (igniter == ao_igniter_drogue && ao_config.apogee_delay) - ao_delay(AO_SEC_TO_TICKS(ao_config.apogee_delay)); - - ao_igniter_fire(igniter); - ao_delay(AO_IGNITER_CHARGE_TIME); - ao_ignition[igniter].fired = 1; - } - } - } -} - -#endif - -void -ao_ignite_manual(void) -{ - ao_cmd_white(); - if (!ao_match_word("DoIt")) - return; - ao_cmd_white(); -#if HAS_IGNITE - if (ao_cmd_lex_c == 'm' && ao_match_word("main")) { - ao_igniter_fire(ao_igniter_main); - return; - } - if (ao_cmd_lex_c == 'd' && ao_match_word("drogue")) { - ao_igniter_fire(ao_igniter_drogue); - return; - } -#endif -#if AO_PYRO_NUM - if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') { - ao_pyro_manual(ao_cmd_lex_c - '0'); - return; - } -#endif - ao_cmd_status = ao_cmd_syntax_error; -} - -__code char * __code ao_igniter_status_names[] = { - "unknown", "ready", "active", "open" -}; - -#if HAS_IGNITE -void -ao_ignite_print_status(enum ao_igniter igniter, __code char *name) __reentrant -{ - enum ao_igniter_status status = ao_igniter_status(igniter); - printf("Igniter: %6s Status: %s\n", - name, - ao_igniter_status_names[status]); -} -#endif - -void -ao_ignite_test(void) -{ -#if HAS_IGNITE - ao_ignite_print_status(ao_igniter_drogue, "drogue"); - ao_ignite_print_status(ao_igniter_main, "main"); -#endif -#if AO_PYRO_NUM - ao_pyro_print_status(); -#endif -} - -__code struct ao_cmds ao_ignite_cmds[] = { - { ao_ignite_manual, "i {main|drogue}\0Fire igniter. is doit with D&I" }, - { ao_ignite_test, "t\0Test igniter" }, - { 0, NULL }, -}; - -#if HAS_IGNITE -__xdata struct ao_task ao_igniter_task; - -void -ao_ignite_set_pins(void) -{ - ao_enable_output(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, AO_IGNITER_DROGUE, 0); - ao_enable_output(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, 0); -} -#endif - -void -ao_igniter_init(void) -{ -#if HAS_IGNITE - ao_ignite_set_pins(); - ao_add_task(&ao_igniter_task, ao_igniter, "igniter"); -#endif - ao_cmd_register(&ao_ignite_cmds[0]); -} diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c deleted file mode 100644 index aa23dbe0..00000000 --- a/src/core/ao_int64.c +++ /dev/null @@ -1,158 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include - -__pdata ao_int64_t *__data ao_64r, *__data ao_64a, *__data ao_64b; - -void ao_plus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR { - __LOCAL uint32_t t; - - r->high = a->high + b->high; - t = a->low + b->low; - if (t < a->low) - r->high++; - r->low = t; -} - -void ao_minus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR { - __LOCAL uint32_t t; - - r->high = a->high - b->high; - t = a->low - b->low; - if (t > a->low) - r->high--; - r->low = t; -} - -void ao_rshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR { - if (d < 32) { - r->low = a->low >> d; - if (d) - r->low |= a->high << (32 - d); - r->high = (int32_t) a->high >> d; - } else { - d &= 0x1f; - r->low = (int32_t) a->high >> d; - r->high = 0; - } -} - -void ao_lshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR { - if (d < 32) { - r->high = a->high << d; - if (d) - r->high |= a->low >> (32 - d); - r->low = a->low << d; - } else { - d &= 0x1f; - r->high = a->low << d; - r->low = 0; - } -} - -static void ao_umul64_32_32(__ARG ao_int64_t *r, uint32_t a, uint32_t b) __reentrant { - __LOCAL uint32_t s; - __LOCAL ao_int64_t t; - r->low = (uint32_t) (uint16_t) a * (uint16_t) b; - r->high = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16); - - s = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b; - - t.high = s >> 16; - t.low = s << 16; - ao_plus64(r, r, &t); - - s = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16); - - t.high = s >> 16; - t.low = s << 16; - ao_plus64(r, r, &t); -} - -void ao_neg64(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR { - r->high = ~a->high; - if (!(r->low = ~a->low + 1)) - r->high++; -} - -void ao_mul64_32_32(__ARG ao_int64_t *r, int32_t a, int32_t b) __FATTR { - uint8_t negative = 0; - - if (a < 0) { - a = -a; - ++negative; - } - if (b < 0) { - b = -b; - --negative; - } - ao_umul64_32_32(r, a, b); - if (negative) - ao_neg64(r, r); -} - -static void ao_umul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __reentrant { - __LOCAL ao_int64_t r2, r3; - - ao_umul64_32_32(&r2, a->high, b->low); - ao_umul64_32_32(&r3, a->low, b->high); - ao_umul64_32_32(r, a->low, b->low); - - r->high += r2.low + r3.low; -} - -static __ARG ao_int64_t ap, bp; - -void ao_mul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __FATTR { - uint8_t negative = 0; - - if (ao_int64_negativep(a)) { - ao_neg64(&ap, a); - a = ≈ - ++negative; - } - if (ao_int64_negativep(b)) { - ao_neg64(&bp, b); - b = &bp; - --negative; - } - ao_umul64(r, a, b); - if (negative) - ao_neg64(r, r); -} - -static void ao_umul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, uint16_t b) __reentrant { - __LOCAL uint32_t h; - - h = a->high * b; - ao_umul64_32_32(r, a->low, b); - r->high += h; -} - -void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR { - uint8_t negative = 0; - - if ((int32_t) a->high < 0) { - ao_neg64(&ap, a); - a = ≈ - negative++; - } else - ao_umul64_64_16(r, a, b); - if (negative) - ao_neg64(r, r); -} diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h deleted file mode 100644 index b16db58c..00000000 --- a/src/core/ao_int64.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_INT64_H_ -#define _AO_INT64_H_ - -#include - -typedef struct { - uint32_t high; - uint32_t low; -} ao_int64_t; - -#define __FATTR -#define __ARG __pdata -#define __LOCAL static __pdata - -void ao_plus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR; -void ao_minus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR; -void ao_neg64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a) __FATTR; -void ao_rshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR; -void ao_lshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR; -void ao_mul64_32_32(__ARG ao_int64_t *r, __ARG int32_t a, __ARG int32_t b) __FATTR; -void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR; -void ao_mul64(__ARG ao_int64_t * __ARG r, __ARG ao_int64_t * __ARG a, __ARG ao_int64_t *__ARG b) __FATTR; - -#define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a)) -#define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b)) - -#define ao_cast64(a) (((int64_t) (a)->high << 32) | (a)->low) - -#define ao_int64_negativep(a) (((int32_t) (a)->high) < 0) - -#endif /* _AO_INT64_H_ */ diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c deleted file mode 100644 index 9aea1f14..00000000 --- a/src/core/ao_kalman.c +++ /dev/null @@ -1,295 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_FLIGHT_TEST -#include "ao.h" -#include "ao_flight.h" -#endif - -#include "ao_sample.h" -#include "ao_kalman.h" - - -static __pdata int32_t ao_k_height; -static __pdata int32_t ao_k_speed; -static __pdata int32_t ao_k_accel; - -#define AO_K_STEP_100 to_fix16(0.01) -#define AO_K_STEP_2_2_100 to_fix16(0.00005) - -#define AO_K_STEP_10 to_fix16(0.1) -#define AO_K_STEP_2_2_10 to_fix16(0.005) - -#define AO_K_STEP_1 to_fix16(1) -#define AO_K_STEP_2_2_1 to_fix16(0.5) - -__pdata int16_t ao_height; -__pdata int16_t ao_speed; -__pdata int16_t ao_accel; -__xdata int16_t ao_max_height; -static __pdata int32_t ao_avg_height_scaled; -__xdata int16_t ao_avg_height; - -__pdata int16_t ao_error_h; -__pdata int16_t ao_error_h_sq_avg; - -#if HAS_ACCEL -__pdata int16_t ao_error_a; -#endif - -static void -ao_kalman_predict(void) -{ -#ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 + - (int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1; - - return; - } - if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 + - (int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10; - - return; - } - if (ao_flight_debug) { - printf ("predict speed %g + (%g * %g) = %g\n", - ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0, - (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); - } -#endif - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 + - (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100; -} - -static void -ao_kalman_err_height(void) -{ - int16_t e; - int16_t height_distrust; -#if HAS_ACCEL - int16_t speed_distrust; -#endif - - ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16); - - e = ao_error_h; - if (e < 0) - e = -e; - if (e > 127) - e = 127; -#if HAS_ACCEL - ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2; - ao_error_h_sq_avg += (e * e) >> 2; -#else - ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4; - ao_error_h_sq_avg += (e * e) >> 4; -#endif - - if (ao_flight_state >= ao_flight_drogue) - return; - height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT; -#if HAS_ACCEL - /* speed is stored * 16, but we need to ramp between 200 and 328, so - * we want to multiply by 2. The result is a shift by 3. - */ - speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1); - if (speed_distrust <= 0) - speed_distrust = 0; - else if (speed_distrust > height_distrust) - height_distrust = speed_distrust; -#endif - if (height_distrust > 0) { -#ifdef AO_FLIGHT_TEST - int old_ao_error_h = ao_error_h; -#endif - if (height_distrust > 0x100) - height_distrust = 0x100; - ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8); -#ifdef AO_FLIGHT_TEST - if (ao_flight_debug) { - printf("over height %g over speed %g distrust: %g height: error %d -> %d\n", - (double) (ao_sample_alt - AO_MAX_BARO_HEIGHT), - (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) / 16.0, - height_distrust / 256.0, - old_ao_error_h, ao_error_h); - } -#endif - } -} - -static void -ao_kalman_correct_baro(void) -{ - ao_kalman_err_height(); -#ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_1 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_1 * ao_error_h; - return; - } - if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_10 * ao_error_h; - return; - } -#endif - ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_100 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_100 * ao_error_h; -} - -#if HAS_ACCEL - -static void -ao_kalman_err_accel(void) -{ - int32_t accel; - - accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; - - /* Can't use ao_accel here as it is the pre-prediction value still */ - ao_error_a = (accel - ao_k_accel) >> 16; -} - -#ifndef FORCE_ACCEL -static void -ao_kalman_correct_both(void) -{ - ao_kalman_err_height(); - ao_kalman_err_accel(); - -#ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { - if (ao_flight_debug) { - printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", - ao_k_speed / (65536.0 * 16.0), - (double) ao_error_h, AO_BOTH_K10_1 / 65536.0, - (double) ao_error_a, AO_BOTH_K11_1 / 65536.0, - (ao_k_speed + - (int32_t) AO_BOTH_K10_1 * ao_error_h + - (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0)); - } - ao_k_height += - (int32_t) AO_BOTH_K00_1 * ao_error_h + - (int32_t) AO_BOTH_K01_1 * ao_error_a; - ao_k_speed += - (int32_t) AO_BOTH_K10_1 * ao_error_h + - (int32_t) AO_BOTH_K11_1 * ao_error_a; - ao_k_accel += - (int32_t) AO_BOTH_K20_1 * ao_error_h + - (int32_t) AO_BOTH_K21_1 * ao_error_a; - return; - } - if (ao_sample_tick - ao_sample_prev_tick > 5) { - if (ao_flight_debug) { - printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", - ao_k_speed / (65536.0 * 16.0), - (double) ao_error_h, AO_BOTH_K10_10 / 65536.0, - (double) ao_error_a, AO_BOTH_K11_10 / 65536.0, - (ao_k_speed + - (int32_t) AO_BOTH_K10_10 * ao_error_h + - (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); - } - ao_k_height += - (int32_t) AO_BOTH_K00_10 * ao_error_h + - (int32_t) AO_BOTH_K01_10 * ao_error_a; - ao_k_speed += - (int32_t) AO_BOTH_K10_10 * ao_error_h + - (int32_t) AO_BOTH_K11_10 * ao_error_a; - ao_k_accel += - (int32_t) AO_BOTH_K20_10 * ao_error_h + - (int32_t) AO_BOTH_K21_10 * ao_error_a; - return; - } - if (ao_flight_debug) { - printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", - ao_k_speed / (65536.0 * 16.0), - (double) ao_error_h, AO_BOTH_K10_100 / 65536.0, - (double) ao_error_a, AO_BOTH_K11_100 / 65536.0, - (ao_k_speed + - (int32_t) AO_BOTH_K10_100 * ao_error_h + - (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); - } -#endif - ao_k_height += - (int32_t) AO_BOTH_K00_100 * ao_error_h + - (int32_t) AO_BOTH_K01_100 * ao_error_a; - ao_k_speed += - (int32_t) AO_BOTH_K10_100 * ao_error_h + - (int32_t) AO_BOTH_K11_100 * ao_error_a; - ao_k_accel += - (int32_t) AO_BOTH_K20_100 * ao_error_h + - (int32_t) AO_BOTH_K21_100 * ao_error_a; -} - -#else - -static void -ao_kalman_correct_accel(void) -{ - ao_kalman_err_accel(); - - if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a; - ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a; - ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a; - return; - } - ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a; - ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a; - ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a; -} - -#endif /* else FORCE_ACCEL */ -#endif /* HAS_ACCEL */ - -void -ao_kalman(void) -{ - ao_kalman_predict(); -#if HAS_ACCEL - if (ao_flight_state <= ao_flight_coast) { -#ifdef FORCE_ACCEL - ao_kalman_correct_accel(); -#else - ao_kalman_correct_both(); -#endif - } else -#endif - ao_kalman_correct_baro(); - ao_height = from_fix(ao_k_height); - ao_speed = from_fix(ao_k_speed); - ao_accel = from_fix(ao_k_accel); - if (ao_height > ao_max_height) - ao_max_height = ao_height; - ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height; -#ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) - ao_avg_height = (ao_avg_height_scaled + 1) >> 1; - else if (ao_sample_tick - ao_sample_prev_tick > 5) - ao_avg_height = (ao_avg_height_scaled + 7) >> 4; - else -#endif - ao_avg_height = (ao_avg_height_scaled + 63) >> 7; -} diff --git a/src/core/ao_lcd.h b/src/core/ao_lcd.h deleted file mode 100644 index f7e1391a..00000000 --- a/src/core/ao_lcd.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_LCD_H_ -#define _AO_LCD_H_ - -/* ao_lcd.c */ - -void -ao_lcd_putchar(uint8_t d); - -void -ao_lcd_putstring(char *string); - -void -ao_lcd_contrast_set(uint8_t contrast); - -void -ao_lcd_clear(void); - -void -ao_lcd_cursor_on(void); - -void -ao_lcd_cursor_off(void); - -#define AO_LCD_ADDR(row,col) ((row << 6) | (col)) - -void -ao_lcd_goto(uint8_t addr); - -void -ao_lcd_start(void); - -void -ao_lcd_init(void); - -/* ao_lcd_port.c */ - -void -ao_lcd_port_put_nibble(uint8_t rs, uint8_t d); - -void -ao_lcd_port_init(void); - -#endif /* _AO_LCD_H_ */ diff --git a/src/core/ao_led.h b/src/core/ao_led.h deleted file mode 100644 index d9a0914a..00000000 --- a/src/core/ao_led.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_LED_H_ -#define _AO_LED_H_ - -/* - * ao_led.c - */ - -#define AO_LED_NONE 0 - -#ifndef AO_LED_TYPE -#define AO_LED_TYPE uint8_t -#endif - -/* Turn on the specified LEDs */ -void -ao_led_on(AO_LED_TYPE colors); - -/* Turn off the specified LEDs */ -void -ao_led_off(AO_LED_TYPE colors); - -/* Set all of the LEDs to the specified state */ -void -ao_led_set(AO_LED_TYPE colors); - -/* Set all LEDs in 'mask' to the specified state */ -void -ao_led_set_mask(uint8_t colors, uint8_t mask); - -/* Toggle the specified LEDs */ -void -ao_led_toggle(AO_LED_TYPE colors); - -/* Turn on the specified LEDs for the indicated interval */ -void -ao_led_for(AO_LED_TYPE colors, uint16_t ticks) __reentrant; - -/* Initialize the LEDs */ -void -ao_led_init(AO_LED_TYPE enable); - -#endif /* _AO_LED_H_ */ diff --git a/src/core/ao_list.h b/src/core/ao_list.h deleted file mode 100644 index 8a6fa4d9..00000000 --- a/src/core/ao_list.h +++ /dev/null @@ -1,213 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_LIST_H_ -#define _AO_LIST_H_ - -#include - -struct ao_list { - struct ao_list *next, *prev; -}; - -static inline void -ao_list_init(struct ao_list *list) -{ - list->next = list->prev = list; -} - -static inline void -__ao_list_add(struct ao_list *list, struct ao_list *prev, struct ao_list *next) -{ - next->prev = list; - list->next = next; - list->prev = prev; - prev->next = list; -} - -/** - * Insert a new element after the given list head. The new element does not - * need to be initialised as empty list. - * The list changes from: - * head → some element → ... - * to - * head → new element → older element → ... - * - * Example: - * struct foo *newfoo = malloc(...); - * ao_list_add(&newfoo->entry, &bar->list_of_foos); - * - * @param entry The new element to prepend to the list. - * @param head The existing list. - */ -static inline void -ao_list_insert(struct ao_list *entry, struct ao_list *head) -{ - __ao_list_add(entry, head, head->next); -} - -/** - * Append a new element to the end of the list given with this list head. - * - * The list changes from: - * head → some element → ... → lastelement - * to - * head → some element → ... → lastelement → new element - * - * Example: - * struct foo *newfoo = malloc(...); - * ao_list_append(&newfoo->entry, &bar->list_of_foos); - * - * @param entry The new element to prepend to the list. - * @param head The existing list. - */ -static inline void -ao_list_append(struct ao_list *entry, struct ao_list *head) -{ - __ao_list_add(entry, head->prev, head); -} - -static inline void -__ao_list_del(struct ao_list *prev, struct ao_list *next) -{ - next->prev = prev; - prev->next = next; -} - -/** - * Remove the element from the list it is in. Using this function will reset - * the pointers to/from this element so it is removed from the list. It does - * NOT free the element itself or manipulate it otherwise. - * - * Using ao_list_del on a pure list head (like in the example at the top of - * this file) will NOT remove the first element from - * the list but rather reset the list as empty list. - * - * Example: - * ao_list_del(&foo->entry); - * - * @param entry The element to remove. - */ -static inline void -ao_list_del(struct ao_list *entry) -{ - __ao_list_del(entry->prev, entry->next); - ao_list_init(entry); -} - -/** - * Check if the list is empty. - * - * Example: - * ao_list_is_empty(&bar->list_of_foos); - * - * @return True if the list contains one or more elements or False otherwise. - */ -static inline uint8_t -ao_list_is_empty(struct ao_list *head) -{ - return head->next == head; -} - -/** - * Returns a pointer to the container of this list element. - * - * Example: - * struct foo* f; - * f = container_of(&foo->entry, struct foo, entry); - * assert(f == foo); - * - * @param ptr Pointer to the struct ao_list. - * @param type Data type of the list element. - * @param member Member name of the struct ao_list field in the list element. - * @return A pointer to the data struct containing the list head. - */ -#define ao_container_of(ptr, type, member) \ - ((type *)((char *)(ptr) - offsetof(type, member))) - -/** - * Alias of ao_container_of - */ -#define ao_list_entry(ptr, type, member) \ - ao_container_of(ptr, type, member) - -/** - * Retrieve the first list entry for the given list pointer. - * - * Example: - * struct foo *first; - * first = ao_list_first_entry(&bar->list_of_foos, struct foo, list_of_foos); - * - * @param ptr The list head - * @param type Data type of the list element to retrieve - * @param member Member name of the struct ao_list field in the list element. - * @return A pointer to the first list element. - */ -#define ao_list_first_entry(ptr, type, member) \ - ao_list_entry((ptr)->next, type, member) - -/** - * Retrieve the last list entry for the given listpointer. - * - * Example: - * struct foo *first; - * first = ao_list_last_entry(&bar->list_of_foos, struct foo, list_of_foos); - * - * @param ptr The list head - * @param type Data type of the list element to retrieve - * @param member Member name of the struct ao_list field in the list element. - * @return A pointer to the last list element. - */ -#define ao_list_last_entry(ptr, type, member) \ - ao_list_entry((ptr)->prev, type, member) - -/** - * Loop through the list given by head and set pos to struct in the list. - * - * Example: - * struct foo *iterator; - * ao_list_for_each_entry(iterator, &bar->list_of_foos, entry) { - * [modify iterator] - * } - * - * This macro is not safe for node deletion. Use ao_list_for_each_entry_safe - * instead. - * - * @param pos Iterator variable of the type of the list elements. - * @param head List head - * @param member Member name of the struct ao_list in the list elements. - * - */ -#define ao_list_for_each_entry(pos, head, type, member) \ - for (pos = ao_container_of((head)->next, type, member); \ - &pos->member != (head); \ - pos = ao_container_of(pos->member.next, type, member)) - -/** - * Loop through the list, keeping a backup pointer to the element. This - * macro allows for the deletion of a list element while looping through the - * list. - * - * See ao_list_for_each_entry for more details. - */ -#define ao_list_for_each_entry_safe(pos, tmp, head, type, member) \ - for ((pos = ao_container_of((head)->next, type, member)), \ - (tmp = ao_container_of(pos->member.next, type, member)); \ - &pos->member != (head); \ - (pos = tmp), (tmp = ao_container_of(pos->member.next, type, member))) - -#endif /* _AO_LIST_H_ */ diff --git a/src/core/ao_log.c b/src/core/ao_log.c deleted file mode 100644 index 20febefe..00000000 --- a/src/core/ao_log.c +++ /dev/null @@ -1,291 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include -#include - -__pdata uint32_t ao_log_current_pos; -__pdata uint32_t ao_log_end_pos; -__pdata uint32_t ao_log_start_pos; -__xdata uint8_t ao_log_running; -__pdata enum ao_flight_state ao_log_state; -__xdata uint16_t ao_flight_number; - -void -ao_log_flush(void) -{ - ao_storage_flush(); -} - -/* - * When erasing a flight log, make sure the config block - * has an up-to-date version of the current flight number - */ - -struct ao_log_erase { - uint8_t unused; - uint16_t flight; -}; - -static __xdata struct ao_log_erase erase; - -#define LOG_MAX_ERASE 16 - -static uint32_t -ao_log_erase_pos(uint8_t i) -{ - return i * sizeof (struct ao_log_erase) + AO_CONFIG_MAX_SIZE; -} - -void -ao_log_write_erase(uint8_t pos) -{ - erase.unused = 0x00; - erase.flight = ao_flight_number; - ao_config_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); - ao_config_flush(); -} - -static void -ao_log_read_erase(uint8_t pos) -{ - ao_config_read(ao_log_erase_pos(pos), &erase, sizeof (erase)); -} - - -static void -ao_log_erase_mark(void) -{ - uint8_t i; - - for (i = 0; i < LOG_MAX_ERASE; i++) { - ao_log_read_erase(i); - if (erase.unused == 0 && erase.flight == ao_flight_number) - return; - if (erase.unused == 0xff) { - ao_log_write_erase(i); - return; - } - } - ao_config_put(); -} - -static uint8_t -ao_log_slots() -{ - return (uint8_t) (ao_storage_log_max / ao_config.flight_log_max); -} - -uint32_t -ao_log_pos(uint8_t slot) -{ - return ((slot) * ao_config.flight_log_max); -} - -static uint16_t -ao_log_max_flight(void) -{ - uint8_t log_slot; - uint8_t log_slots; - uint16_t log_flight; - uint16_t max_flight = 0; - - /* Scan the log space looking for the biggest flight number */ - log_slots = ao_log_slots(); - for (log_slot = 0; log_slot < log_slots; log_slot++) { - log_flight = ao_log_flight(log_slot); - if (!log_flight) - continue; - if (max_flight == 0 || (int16_t) (log_flight - max_flight) > 0) - max_flight = log_flight; - } - return max_flight; -} - -void -ao_log_scan(void) __reentrant -{ - uint8_t log_slot; - uint8_t log_slots; - uint8_t log_want; - - ao_config_get(); - - ao_flight_number = ao_log_max_flight(); - if (ao_flight_number) - if (++ao_flight_number == 0) - ao_flight_number = 1; - - /* Now look through the log of flight numbers from erase operations and - * see if the last one is bigger than what we found above - */ - for (log_slot = LOG_MAX_ERASE; log_slot-- > 0;) { - ao_log_read_erase(log_slot); - if (erase.unused == 0) { - if (ao_flight_number == 0 || - (int16_t) (erase.flight - ao_flight_number) > 0) - ao_flight_number = erase.flight; - break; - } - } - if (ao_flight_number == 0) - ao_flight_number = 1; - - /* With a flight number in hand, find a place to write a new log, - * use the target flight number to index the available log slots so - * that we write logs to each spot about the same number of times. - */ - - /* Find a log slot for the next flight, if available */ - ao_log_current_pos = ao_log_end_pos = 0; - log_slots = ao_log_slots(); - log_want = (ao_flight_number - 1) % log_slots; - log_slot = log_want; - do { - if (ao_log_flight(log_slot) == 0) { - ao_log_current_pos = ao_log_pos(log_slot); - ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; - break; - } - if (++log_slot >= log_slots) - log_slot = 0; - } while (log_slot != log_want); - - ao_wakeup(&ao_flight_number); -} - -void -ao_log_start(void) -{ - /* start logging */ - ao_log_running = 1; - ao_wakeup(&ao_log_running); -} - -void -ao_log_stop(void) -{ - ao_log_running = 0; - ao_log_flush(); -} - -uint8_t -ao_log_present(void) -{ - return ao_log_max_flight() != 0; -} - -uint8_t -ao_log_full(void) -{ - return ao_log_current_pos == ao_log_end_pos; -} - -#if HAS_ADC -static __xdata struct ao_task ao_log_task; -#endif - -void -ao_log_list(void) __reentrant -{ - uint8_t slot; - uint8_t slots; - uint16_t flight; - - slots = ao_log_slots(); - for (slot = 0; slot < slots; slot++) - { - flight = ao_log_flight(slot); - if (flight) - printf ("flight %d start %x end %x\n", - flight, - (uint16_t) (ao_log_pos(slot) >> 8), - (uint16_t) (ao_log_pos(slot+1) >> 8)); - } - printf ("done\n"); -} - -void -ao_log_delete(void) __reentrant -{ - uint8_t slot; - uint8_t slots; - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - - slots = ao_log_slots(); - /* Look for the flight log matching the requested flight */ - if (ao_cmd_lex_i) { - for (slot = 0; slot < slots; slot++) { - if (ao_log_flight(slot) == ao_cmd_lex_i) { - ao_log_erase_mark(); - ao_log_current_pos = ao_log_pos(slot); - ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; - while (ao_log_current_pos < ao_log_end_pos) { - uint8_t i; - static __xdata uint8_t b; - - /* - * Check to see if we've reached the end of - * the used memory to avoid re-erasing the same - * memory over and over again - */ - for (i = 0; i < 16; i++) { - if (ao_storage_read(ao_log_current_pos + i, &b, 1)) - if (b != 0xff) - break; - } - if (i == 16) - break; - ao_storage_erase(ao_log_current_pos); - ao_log_current_pos += ao_storage_block; - } - puts("Erased"); - return; - } - } - } - printf("No such flight: %d\n", ao_cmd_lex_i); -} - -__code struct ao_cmds ao_log_cmds[] = { - { ao_log_list, "l\0List logs" }, - { ao_log_delete, "d \0Delete flight" }, - { 0, NULL }, -}; - -void -ao_log_init(void) -{ - ao_log_running = 0; - - /* For now, just log the flight starting at the begining of eeprom */ - ao_log_state = ao_flight_invalid; - - ao_cmd_register(&ao_log_cmds[0]); - -#ifndef HAS_ADC -#error Define HAS_ADC for ao_log.c -#endif -#if HAS_ADC - /* Create a task to log events to eeprom */ - ao_add_task(&ao_log_task, ao_log, "log"); -#endif -} diff --git a/src/core/ao_log.h b/src/core/ao_log.h deleted file mode 100644 index 09f31188..00000000 --- a/src/core/ao_log.h +++ /dev/null @@ -1,386 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_LOG_H_ -#define _AO_LOG_H_ - -#include - -/* - * ao_log.c - */ - -/* We record flight numbers in the first record of - * the log. Tasks may wait for this to be initialized - * by sleeping on this variable. - */ -extern __xdata uint16_t ao_flight_number; - -extern __pdata uint32_t ao_log_current_pos; -extern __pdata uint32_t ao_log_end_pos; -extern __pdata uint32_t ao_log_start_pos; -extern __xdata uint8_t ao_log_running; -extern __pdata enum ao_flight_state ao_log_state; - -/* required functions from the underlying log system */ - -#define AO_LOG_FORMAT_UNKNOWN 0 /* unknown; altosui will have to guess */ -#define AO_LOG_FORMAT_FULL 1 /* 8 byte typed log records */ -#define AO_LOG_FORMAT_TINY 2 /* two byte state/baro records */ -#define AO_LOG_FORMAT_TELEMETRY 3 /* 32 byte ao_telemetry records */ -#define AO_LOG_FORMAT_TELESCIENCE 4 /* 32 byte typed telescience records */ -#define AO_LOG_FORMAT_TELEMEGA 5 /* 32 byte typed telemega records */ -#define AO_LOG_FORMAT_EASYMINI 6 /* 16-byte MS5607 baro only, 3.0V supply */ -#define AO_LOG_FORMAT_TELEMETRUM 7 /* 16-byte typed telemetrum records */ -#define AO_LOG_FORMAT_TELEMINI 8 /* 16-byte MS5607 baro only, 3.3V supply */ -#define AO_LOG_FORMAT_NONE 127 /* No log at all */ - -extern __code uint8_t ao_log_format; - -/* Return the flight number from the given log slot, 0 if none */ -uint16_t -ao_log_flight(uint8_t slot); - -/* Flush the log */ -void -ao_log_flush(void); - -/* Logging thread main routine */ -void -ao_log(void); - -/* functions provided in ao_log.c */ - -/* Figure out the current flight number */ -void -ao_log_scan(void) __reentrant; - -/* Return the position of the start of the given log slot */ -uint32_t -ao_log_pos(uint8_t slot); - -/* Start logging to eeprom */ -void -ao_log_start(void); - -/* Stop logging */ -void -ao_log_stop(void); - -/* Initialize the logging system */ -void -ao_log_init(void); - -/* Write out the current flight number to the erase log */ -void -ao_log_write_erase(uint8_t pos); - -/* Returns true if there are any logs stored in eeprom */ -uint8_t -ao_log_present(void); - -/* Returns true if there is no more storage space available */ -uint8_t -ao_log_full(void); - -/* - * ao_log_big.c - */ - -/* - * The data log is recorded in the eeprom as a sequence - * of data packets. - * - * Each packet starts with a 4-byte header that has the - * packet type, the packet checksum and the tick count. Then - * they all contain 2 16 bit values which hold packet-specific - * data. - * - * For each flight, the first packet - * is FLIGHT packet, indicating the serial number of the - * device and a unique number marking the number of flights - * recorded by this device. - * - * During flight, data from the accelerometer and barometer - * are recorded in SENSOR packets, using the raw 16-bit values - * read from the A/D converter. - * - * Also during flight, but at a lower rate, the deployment - * sensors are recorded in DEPLOY packets. The goal here is to - * detect failure in the deployment circuits. - * - * STATE packets hold state transitions as the flight computer - * transitions through different stages of the flight. - */ -#define AO_LOG_FLIGHT 'F' -#define AO_LOG_SENSOR 'A' -#define AO_LOG_TEMP_VOLT 'T' -#define AO_LOG_DEPLOY 'D' -#define AO_LOG_STATE 'S' -#define AO_LOG_GPS_TIME 'G' -#define AO_LOG_GPS_LAT 'N' -#define AO_LOG_GPS_LON 'W' -#define AO_LOG_GPS_ALT 'H' -#define AO_LOG_GPS_SAT 'V' -#define AO_LOG_GPS_DATE 'Y' -#define AO_LOG_GPS_POS 'P' - -#define AO_LOG_POS_NONE (~0UL) - -struct ao_log_record { - char type; /* 0 */ - uint8_t csum; /* 1 */ - uint16_t tick; /* 2 */ - union { - struct { - int16_t ground_accel; /* 4 */ - uint16_t flight; /* 6 */ - } flight; - struct { - int16_t accel; /* 4 */ - int16_t pres; /* 6 */ - } sensor; - struct { - int16_t temp; - int16_t v_batt; - } temp_volt; - struct { - int16_t drogue; - int16_t main; - } deploy; - struct { - uint16_t state; - uint16_t reason; - } state; - struct { - uint8_t hour; - uint8_t minute; - uint8_t second; - uint8_t flags; - } gps_time; - int32_t gps_latitude; - int32_t gps_longitude; - struct { - int16_t altitude; - uint16_t unused; - } gps_altitude; - struct { - uint16_t svid; - uint8_t unused; - uint8_t c_n; - } gps_sat; - struct { - uint8_t year; - uint8_t month; - uint8_t day; - uint8_t extra; - } gps_date; - struct { - uint16_t d0; - uint16_t d1; - } anon; - } u; -}; - -struct ao_log_mega { - char type; /* 0 */ - uint8_t csum; /* 1 */ - uint16_t tick; /* 2 */ - union { /* 4 */ - /* AO_LOG_FLIGHT */ - struct { - uint16_t flight; /* 4 */ - int16_t ground_accel; /* 6 */ - uint32_t ground_pres; /* 8 */ - int16_t ground_accel_along; /* 16 */ - int16_t ground_accel_across; /* 12 */ - int16_t ground_accel_through; /* 14 */ - int16_t ground_roll; /* 18 */ - int16_t ground_pitch; /* 20 */ - int16_t ground_yaw; /* 22 */ - } flight; /* 24 */ - /* AO_LOG_STATE */ - struct { - uint16_t state; - uint16_t reason; - } state; - /* AO_LOG_SENSOR */ - struct { - uint32_t pres; /* 4 */ - uint32_t temp; /* 8 */ - int16_t accel_x; /* 12 */ - int16_t accel_y; /* 14 */ - int16_t accel_z; /* 16 */ - int16_t gyro_x; /* 18 */ - int16_t gyro_y; /* 20 */ - int16_t gyro_z; /* 22 */ - int16_t mag_x; /* 24 */ - int16_t mag_y; /* 26 */ - int16_t mag_z; /* 28 */ - int16_t accel; /* 30 */ - } sensor; /* 32 */ - /* AO_LOG_TEMP_VOLT */ - struct { - int16_t v_batt; /* 4 */ - int16_t v_pbatt; /* 6 */ - int16_t n_sense; /* 8 */ - int16_t sense[10]; /* 10 */ - uint16_t pyro; /* 30 */ - } volt; /* 32 */ - /* AO_LOG_GPS_TIME */ - struct { - int32_t latitude; /* 4 */ - int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ - uint8_t hour; /* 14 */ - uint8_t minute; /* 15 */ - uint8_t second; /* 16 */ - uint8_t flags; /* 17 */ - uint8_t year; /* 18 */ - uint8_t month; /* 19 */ - uint8_t day; /* 20 */ - uint8_t course; /* 21 */ - uint16_t ground_speed; /* 22 */ - int16_t climb_rate; /* 24 */ - uint8_t pdop; /* 26 */ - uint8_t hdop; /* 27 */ - uint8_t vdop; /* 28 */ - uint8_t mode; /* 29 */ - } gps; /* 30 */ - /* AO_LOG_GPS_SAT */ - struct { - uint16_t channels; /* 4 */ - struct { - uint8_t svid; - uint8_t c_n; - } sats[12]; /* 6 */ - } gps_sat; /* 30 */ - } u; -}; - -struct ao_log_metrum { - char type; /* 0 */ - uint8_t csum; /* 1 */ - uint16_t tick; /* 2 */ - union { /* 4 */ - /* AO_LOG_FLIGHT */ - struct { - uint16_t flight; /* 4 */ - int16_t ground_accel; /* 6 */ - uint32_t ground_pres; /* 8 */ - uint32_t ground_temp; /* 12 */ - } flight; /* 16 */ - /* AO_LOG_STATE */ - struct { - uint16_t state; /* 4 */ - uint16_t reason; /* 6 */ - } state; /* 8 */ - /* AO_LOG_SENSOR */ - struct { - uint32_t pres; /* 4 */ - uint32_t temp; /* 8 */ - int16_t accel; /* 12 */ - } sensor; /* 14 */ - /* AO_LOG_TEMP_VOLT */ - struct { - int16_t v_batt; /* 4 */ - int16_t sense_a; /* 6 */ - int16_t sense_m; /* 8 */ - } volt; /* 10 */ - /* AO_LOG_GPS_POS */ - struct { - int32_t latitude; /* 4 */ - int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ - } gps; /* 14 */ - /* AO_LOG_GPS_TIME */ - struct { - uint8_t hour; /* 4 */ - uint8_t minute; /* 5 */ - uint8_t second; /* 6 */ - uint8_t flags; /* 7 */ - uint8_t year; /* 8 */ - uint8_t month; /* 9 */ - uint8_t day; /* 10 */ - uint8_t pad; /* 11 */ - } gps_time; /* 12 */ - /* AO_LOG_GPS_SAT (up to three packets) */ - struct { - uint8_t channels; /* 4 */ - uint8_t more; /* 5 */ - struct { - uint8_t svid; - uint8_t c_n; - } sats[4]; /* 6 */ - } gps_sat; /* 14 */ - uint8_t raw[12]; /* 4 */ - } u; /* 16 */ -}; - -struct ao_log_mini { - char type; /* 0 */ - uint8_t csum; /* 1 */ - uint16_t tick; /* 2 */ - union { /* 4 */ - /* AO_LOG_FLIGHT */ - struct { - uint16_t flight; /* 4 */ - uint16_t r6; - uint32_t ground_pres; /* 8 */ - } flight; - /* AO_LOG_STATE */ - struct { - uint16_t state; /* 4 */ - uint16_t reason; /* 6 */ - } state; - /* AO_LOG_SENSOR */ - struct { - uint8_t pres[3]; /* 4 */ - uint8_t temp[3]; /* 7 */ - int16_t sense_a; /* 10 */ - int16_t sense_m; /* 12 */ - int16_t v_batt; /* 14 */ - } sensor; /* 16 */ - } u; /* 16 */ -}; /* 16 */ - -#define ao_log_pack24(dst,value) do { \ - (dst)[0] = (value); \ - (dst)[1] = (value) >> 8; \ - (dst)[2] = (value) >> 16; \ - } while (0) - -/* Write a record to the eeprom log */ -uint8_t -ao_log_data(__xdata struct ao_log_record *log) __reentrant; - -uint8_t -ao_log_mega(__xdata struct ao_log_mega *log) __reentrant; - -uint8_t -ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant; - -uint8_t -ao_log_mini(__xdata struct ao_log_mini *log) __reentrant; - -void -ao_log_flush(void); - -void -ao_gps_report_metrum_init(void); - -#endif /* _AO_LOG_H_ */ diff --git a/src/core/ao_log_big.c b/src/core/ao_log_big.c deleted file mode 100644 index db01f46c..00000000 --- a/src/core/ao_log_big.c +++ /dev/null @@ -1,162 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -static __xdata uint8_t ao_log_mutex; -static __xdata struct ao_log_record log; - -__code uint8_t ao_log_format = AO_LOG_FORMAT_FULL; - -static uint8_t -ao_log_csum(__xdata uint8_t *b) __reentrant -{ - uint8_t sum = 0x5a; - uint8_t i; - - for (i = 0; i < sizeof (struct ao_log_record); i++) - sum += *b++; - return -sum; -} - -uint8_t -ao_log_data(__xdata struct ao_log_record *log) __reentrant -{ - uint8_t wrote = 0; - /* set checksum */ - log->csum = 0; - log->csum = ao_log_csum((__xdata uint8_t *) log); - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - log, - sizeof (struct ao_log_record)); - ao_log_current_pos += sizeof (struct ao_log_record); - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_dump_check_data(void) -{ - if (ao_log_csum((uint8_t *) &log) != 0) - return 0; - return 1; -} - -static __data uint8_t ao_log_data_pos; - -/* a hack to make sure that ao_log_records fill the eeprom block in even units */ -typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_record))] ; - -#ifndef AO_SENSOR_INTERVAL_ASCENT -#define AO_SENSOR_INTERVAL_ASCENT 1 -#define AO_SENSOR_INTERVAL_DESCENT 10 -#define AO_OTHER_INTERVAL 32 -#endif - -void -ao_log(void) -{ - __pdata uint16_t next_sensor, next_other; - - ao_storage_setup(); - - ao_log_scan(); - - while (!ao_log_running) - ao_sleep(&ao_log_running); - - log.type = AO_LOG_FLIGHT; - log.tick = ao_sample_tick; -#if HAS_ACCEL - log.u.flight.ground_accel = ao_ground_accel; -#endif - log.u.flight.flight = ao_flight_number; - ao_log_data(&log); - - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_sample_data); - next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; - ao_log_state = ao_flight_startup; - for (;;) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_sample_data) { - log.tick = ao_data_ring[ao_log_data_pos].tick; - if ((int16_t) (log.tick - next_sensor) >= 0) { - log.type = AO_LOG_SENSOR; - log.u.sensor.accel = ao_data_ring[ao_log_data_pos].adc.accel; - log.u.sensor.pres = ao_data_ring[ao_log_data_pos].adc.pres; - ao_log_data(&log); - if (ao_log_state <= ao_flight_coast) - next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; - else - next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; - } - if ((int16_t) (log.tick - next_other) >= 0) { - log.type = AO_LOG_TEMP_VOLT; - log.u.temp_volt.temp = ao_data_ring[ao_log_data_pos].adc.temp; - log.u.temp_volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; - ao_log_data(&log); - log.type = AO_LOG_DEPLOY; - log.u.deploy.drogue = ao_data_ring[ao_log_data_pos].adc.sense_d; - log.u.deploy.main = ao_data_ring[ao_log_data_pos].adc.sense_m; - ao_log_data(&log); - next_other = log.tick + AO_OTHER_INTERVAL; - } - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } - /* Write state change to EEPROM */ - if (ao_flight_state != ao_log_state) { - ao_log_state = ao_flight_state; - log.type = AO_LOG_STATE; - log.tick = ao_sample_tick; - log.u.state.state = ao_log_state; - log.u.state.reason = 0; - ao_log_data(&log); - - if (ao_log_state == ao_flight_landed) - ao_log_stop(); - } - - /* Wait for a while */ - ao_delay(AO_MS_TO_TICKS(100)); - - /* Stop logging when told to */ - while (!ao_log_running) - ao_sleep(&ao_log_running); - } -} - -uint16_t -ao_log_flight(uint8_t slot) -{ - if (!ao_storage_read(ao_log_pos(slot), - &log, - sizeof (struct ao_log_record))) - return 0; - - if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) - return log.u.flight.flight; - return 0; -} diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c deleted file mode 100644 index 768947d5..00000000 --- a/src/core/ao_log_mega.c +++ /dev/null @@ -1,199 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include -#include -#include - -static __xdata uint8_t ao_log_mutex; -static __xdata struct ao_log_mega log; - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMEGA; - -static uint8_t -ao_log_csum(__xdata uint8_t *b) __reentrant -{ - uint8_t sum = 0x5a; - uint8_t i; - - for (i = 0; i < sizeof (struct ao_log_mega); i++) - sum += *b++; - return -sum; -} - -uint8_t -ao_log_mega(__xdata struct ao_log_mega *log) __reentrant -{ - uint8_t wrote = 0; - /* set checksum */ - log->csum = 0; - log->csum = ao_log_csum((__xdata uint8_t *) log); - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - log, - sizeof (struct ao_log_mega)); - ao_log_current_pos += sizeof (struct ao_log_mega); - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_dump_check_data(void) -{ - if (ao_log_csum((uint8_t *) &log) != 0) - return 0; - return 1; -} - -#if HAS_ADC -static __data uint8_t ao_log_data_pos; - -/* a hack to make sure that ao_log_megas fill the eeprom block in even units */ -typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mega))] ; - -#ifndef AO_SENSOR_INTERVAL_ASCENT -#define AO_SENSOR_INTERVAL_ASCENT 1 -#define AO_SENSOR_INTERVAL_DESCENT 10 -#define AO_OTHER_INTERVAL 32 -#endif - -void -ao_log(void) -{ - __pdata uint16_t next_sensor, next_other; - uint8_t i; - - ao_storage_setup(); - - ao_log_scan(); - - while (!ao_log_running) - ao_sleep(&ao_log_running); - -#if HAS_FLIGHT - log.type = AO_LOG_FLIGHT; - log.tick = ao_sample_tick; -#if HAS_ACCEL - log.u.flight.ground_accel = ao_ground_accel; -#endif -#if HAS_GYRO - log.u.flight.ground_accel_along = ao_ground_accel_along; - log.u.flight.ground_accel_across = ao_ground_accel_across; - log.u.flight.ground_accel_through = ao_ground_accel_through; - log.u.flight.ground_pitch = ao_ground_pitch; - log.u.flight.ground_yaw = ao_ground_yaw; - log.u.flight.ground_roll = ao_ground_roll; -#endif - log.u.flight.ground_pres = ao_ground_pres; - log.u.flight.flight = ao_flight_number; - ao_log_mega(&log); -#endif - - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_data_head); - next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; - ao_log_state = ao_flight_startup; - for (;;) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_data_head) { - log.tick = ao_data_ring[ao_log_data_pos].tick; - if ((int16_t) (log.tick - next_sensor) >= 0) { - log.type = AO_LOG_SENSOR; -#if HAS_MS5607 - log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; - log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; -#endif -#if HAS_MPU6000 - log.u.sensor.accel_x = ao_data_ring[ao_log_data_pos].mpu6000.accel_x; - log.u.sensor.accel_y = ao_data_ring[ao_log_data_pos].mpu6000.accel_y; - log.u.sensor.accel_z = ao_data_ring[ao_log_data_pos].mpu6000.accel_z; - log.u.sensor.gyro_x = ao_data_ring[ao_log_data_pos].mpu6000.gyro_x; - log.u.sensor.gyro_y = ao_data_ring[ao_log_data_pos].mpu6000.gyro_y; - log.u.sensor.gyro_z = ao_data_ring[ao_log_data_pos].mpu6000.gyro_z; -#endif -#if HAS_HMC5883 - log.u.sensor.mag_x = ao_data_ring[ao_log_data_pos].hmc5883.x; - log.u.sensor.mag_y = ao_data_ring[ao_log_data_pos].hmc5883.y; - log.u.sensor.mag_z = ao_data_ring[ao_log_data_pos].hmc5883.z; -#endif - log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); - ao_log_mega(&log); - if (ao_log_state <= ao_flight_coast) - next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; - else - next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; - } - if ((int16_t) (log.tick - next_other) >= 0) { - log.type = AO_LOG_TEMP_VOLT; - log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; - log.u.volt.v_pbatt = ao_data_ring[ao_log_data_pos].adc.v_pbatt; - log.u.volt.n_sense = AO_ADC_NUM_SENSE; - for (i = 0; i < AO_ADC_NUM_SENSE; i++) - log.u.volt.sense[i] = ao_data_ring[ao_log_data_pos].adc.sense[i]; - log.u.volt.pyro = ao_pyro_fired; - ao_log_mega(&log); - next_other = log.tick + AO_OTHER_INTERVAL; - } - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } -#if HAS_FLIGHT - /* Write state change to EEPROM */ - if (ao_flight_state != ao_log_state) { - ao_log_state = ao_flight_state; - log.type = AO_LOG_STATE; - log.tick = ao_time(); - log.u.state.state = ao_log_state; - log.u.state.reason = 0; - ao_log_mega(&log); - - if (ao_log_state == ao_flight_landed) - ao_log_stop(); - } -#endif - - ao_log_flush(); - - /* Wait for a while */ - ao_delay(AO_MS_TO_TICKS(100)); - - /* Stop logging when told to */ - while (!ao_log_running) - ao_sleep(&ao_log_running); - } -} -#endif - -uint16_t -ao_log_flight(uint8_t slot) -{ - if (!ao_storage_read(ao_log_pos(slot), - &log, - sizeof (struct ao_log_mega))) - return 0; - - if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) - return log.u.flight.flight; - return 0; -} diff --git a/src/core/ao_log_metrum.c b/src/core/ao_log_metrum.c deleted file mode 100644 index 9b17adc2..00000000 --- a/src/core/ao_log_metrum.c +++ /dev/null @@ -1,176 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include -#include -#include - -static __xdata uint8_t ao_log_mutex; -static __xdata struct ao_log_metrum log; - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRUM; - -static uint8_t -ao_log_csum(__xdata uint8_t *b) __reentrant -{ - uint8_t sum = 0x5a; - uint8_t i; - - for (i = 0; i < sizeof (struct ao_log_metrum); i++) - sum += *b++; - return -sum; -} - -uint8_t -ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant -{ - uint8_t wrote = 0; - /* set checksum */ - log->csum = 0; - log->csum = ao_log_csum((__xdata uint8_t *) log); - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - log, - sizeof (struct ao_log_metrum)); - ao_log_current_pos += sizeof (struct ao_log_metrum); - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_dump_check_data(void) -{ - if (ao_log_csum((uint8_t *) &log) != 0) - return 0; - return 1; -} - -#if HAS_ADC -static __data uint8_t ao_log_data_pos; - -/* a hack to make sure that ao_log_metrums fill the eeprom block in even units */ -typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_metrum))] ; - -#ifndef AO_SENSOR_INTERVAL_ASCENT -#define AO_SENSOR_INTERVAL_ASCENT 1 -#define AO_SENSOR_INTERVAL_DESCENT 10 -#define AO_OTHER_INTERVAL 32 -#endif - -void -ao_log(void) -{ - __pdata uint16_t next_sensor, next_other; - - ao_storage_setup(); - - ao_log_scan(); - - while (!ao_log_running) - ao_sleep(&ao_log_running); - -#if HAS_FLIGHT - log.type = AO_LOG_FLIGHT; - log.tick = ao_sample_tick; -#if HAS_ACCEL - log.u.flight.ground_accel = ao_ground_accel; -#endif - log.u.flight.ground_pres = ao_ground_pres; - log.u.flight.flight = ao_flight_number; - ao_log_metrum(&log); -#endif - - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_data_head); - next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; - ao_log_state = ao_flight_startup; - for (;;) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_data_head) { - log.tick = ao_data_ring[ao_log_data_pos].tick; - if ((int16_t) (log.tick - next_sensor) >= 0) { - log.type = AO_LOG_SENSOR; -#if HAS_MS5607 - log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; - log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; -#endif -#if HAS_ACCEL - log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); -#endif - ao_log_metrum(&log); - if (ao_log_state <= ao_flight_coast) - next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; - else - next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; - } - if ((int16_t) (log.tick - next_other) >= 0) { - log.type = AO_LOG_TEMP_VOLT; - log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; - log.u.volt.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; - log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; - ao_log_metrum(&log); - next_other = log.tick + AO_OTHER_INTERVAL; - } - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } -#if HAS_FLIGHT - /* Write state change to EEPROM */ - if (ao_flight_state != ao_log_state) { - ao_log_state = ao_flight_state; - log.type = AO_LOG_STATE; - log.tick = ao_time(); - log.u.state.state = ao_log_state; - log.u.state.reason = 0; - ao_log_metrum(&log); - - if (ao_log_state == ao_flight_landed) - ao_log_stop(); - } -#endif - - ao_log_flush(); - - /* Wait for a while */ - ao_delay(AO_MS_TO_TICKS(100)); - - /* Stop logging when told to */ - while (!ao_log_running) - ao_sleep(&ao_log_running); - } -} -#endif - -uint16_t -ao_log_flight(uint8_t slot) -{ - if (!ao_storage_read(ao_log_pos(slot), - &log, - sizeof (struct ao_log_metrum))) - return 0; - - if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) - return log.u.flight.flight; - return 0; -} diff --git a/src/core/ao_log_micro.c b/src/core/ao_log_micro.c deleted file mode 100644 index d665efb5..00000000 --- a/src/core/ao_log_micro.c +++ /dev/null @@ -1,121 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include -#include -#include - -static uint16_t ao_log_offset = STARTING_LOG_OFFSET; - -void -ao_log_micro_save(void) -{ - uint16_t n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t); - ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); - ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); - ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); -} - -void -ao_log_micro_restore(void) -{ - ao_eeprom_read(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); - ao_eeprom_read(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); -} - -void -ao_log_micro_data(void) -{ - uint16_t low_bits = pa; - - if (ao_log_offset < MAX_LOG_OFFSET) { - ao_eeprom_write(ao_log_offset, &low_bits, sizeof (low_bits)); - ao_log_offset += sizeof (low_bits); - } -} - -#define POLY 0x8408 - -static uint16_t -ao_log_micro_crc(uint16_t crc, uint8_t byte) -{ - uint8_t i; - - for (i = 0; i < 8; i++) { - if ((crc & 0x0001) ^ (byte & 0x0001)) - crc = (crc >> 1) ^ POLY; - else - crc = crc >> 1; - byte >>= 1; - } - return crc; -} - -static void -ao_log_hex_nibble(uint8_t b) -{ - if (b < 10) - ao_async_byte('0' + b); - else - ao_async_byte('a' - 10 + b); -} - -static void -ao_log_hex(uint8_t b) -{ - ao_log_hex_nibble(b>>4); - ao_log_hex_nibble(b&0xf); -} - -static void -ao_log_newline(void) -{ - ao_async_byte('\r'); - ao_async_byte('\n'); -} - -void -ao_log_micro_dump(void) -{ - uint16_t n_samples; - uint16_t nbytes; - uint8_t byte; - uint16_t b; - uint16_t crc = 0xffff; - - ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); - if (n_samples == 0xffff) - n_samples = 0; - nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples; - ao_async_start(); - ao_async_byte('M'); - ao_async_byte('P'); - for (b = 0; b < nbytes; b++) { - if ((b & 0xf) == 0) - ao_log_newline(); - ao_eeprom_read(b, &byte, 1); - ao_log_hex(byte); - crc = ao_log_micro_crc(crc, byte); - } - ao_log_newline(); - crc = ~crc; - ao_log_hex(crc >> 8); - ao_log_hex(crc); - ao_log_newline(); - ao_async_stop(); -} diff --git a/src/core/ao_log_micro.h b/src/core/ao_log_micro.h deleted file mode 100644 index 976852ee..00000000 --- a/src/core/ao_log_micro.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_LOG_MICRO_H_ -#define _AO_LOG_MICRO_H_ - -#define PA_GROUND_OFFSET 0 -#define PA_MIN_OFFSET 4 -#define N_SAMPLES_OFFSET 8 -#define STARTING_LOG_OFFSET 10 -#define MAX_LOG_OFFSET 512 - -void -ao_log_micro_save(void); - -void -ao_log_micro_restore(void); - -void -ao_log_micro_data(void); - -void -ao_log_micro_dump(void); - -#endif /* _AO_LOG_MICRO_H_ */ diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c deleted file mode 100644 index 29e3bd9f..00000000 --- a/src/core/ao_log_mini.c +++ /dev/null @@ -1,162 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include -#include -#include - -static __xdata uint8_t ao_log_mutex; -static __xdata struct ao_log_mini log; - -__code uint8_t ao_log_format = AO_LOG_FORMAT; - -static uint8_t -ao_log_csum(__xdata uint8_t *b) __reentrant -{ - uint8_t sum = 0x5a; - uint8_t i; - - for (i = 0; i < sizeof (struct ao_log_mini); i++) - sum += *b++; - return -sum; -} - -uint8_t -ao_log_mini(__xdata struct ao_log_mini *log) __reentrant -{ - uint8_t wrote = 0; - /* set checksum */ - log->csum = 0; - log->csum = ao_log_csum((__xdata uint8_t *) log); - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - log, - sizeof (struct ao_log_mini)); - ao_log_current_pos += sizeof (struct ao_log_mini); - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_dump_check_data(void) -{ - if (ao_log_csum((uint8_t *) &log) != 0) - return 0; - return 1; -} - -static __data uint8_t ao_log_data_pos; - -/* a hack to make sure that ao_log_minis fill the eeprom block in even units */ -typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mini))] ; - -#ifndef AO_SENSOR_INTERVAL_ASCENT -#define AO_SENSOR_INTERVAL_ASCENT 1 -#define AO_SENSOR_INTERVAL_DESCENT 10 -#endif - -void -ao_log(void) -{ - __pdata uint16_t next_sensor; - - ao_storage_setup(); - - ao_log_scan(); - - while (!ao_log_running) - ao_sleep(&ao_log_running); - -#if HAS_FLIGHT - log.type = AO_LOG_FLIGHT; - log.tick = ao_sample_tick; - log.u.flight.flight = ao_flight_number; - log.u.flight.ground_pres = ao_ground_pres; - ao_log_mini(&log); -#endif - - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_data_head); - next_sensor = ao_data_ring[ao_log_data_pos].tick; - ao_log_state = ao_flight_startup; - for (;;) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_data_head) { - log.tick = ao_data_ring[ao_log_data_pos].tick; - if ((int16_t) (log.tick - next_sensor) >= 0) { - log.type = AO_LOG_SENSOR; - ao_log_pack24(log.u.sensor.pres, - ao_data_ring[ao_log_data_pos].ms5607_raw.pres); - ao_log_pack24(log.u.sensor.temp, - ao_data_ring[ao_log_data_pos].ms5607_raw.temp); - log.u.sensor.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; - log.u.sensor.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; - log.u.sensor.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; - ao_log_mini(&log); - if (ao_log_state <= ao_flight_coast) - next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; - else - next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; - } - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } -#if HAS_FLIGHT - /* Write state change to EEPROM */ - if (ao_flight_state != ao_log_state) { - ao_log_state = ao_flight_state; - log.type = AO_LOG_STATE; - log.tick = ao_time(); - log.u.state.state = ao_log_state; - log.u.state.reason = 0; - ao_log_mini(&log); - - if (ao_log_state == ao_flight_landed) - ao_log_stop(); - } -#endif - - ao_log_flush(); - - /* Wait for a while */ - ao_delay(AO_MS_TO_TICKS(100)); - - /* Stop logging when told to */ - while (!ao_log_running) - ao_sleep(&ao_log_running); - } -} - -uint16_t -ao_log_flight(uint8_t slot) -{ - if (!ao_storage_read(ao_log_pos(slot), - &log, - sizeof (struct ao_log_mini))) - return 0; - - if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) - return log.u.flight.flight; - return 0; -} diff --git a/src/core/ao_log_single.c b/src/core/ao_log_single.c deleted file mode 100644 index 3f6235a6..00000000 --- a/src/core/ao_log_single.c +++ /dev/null @@ -1,198 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -/* - * ao_log_single.c - * - * Stores a sequence of fixed-size (32 byte) chunks - * without splitting memory up into separate flights - */ - -#include "ao.h" -#include "ao_product.h" - -static __xdata struct ao_task ao_log_single_task; - -__xdata uint8_t ao_log_running; -__xdata uint8_t ao_log_mutex; -__pdata uint32_t ao_log_start_pos; -__pdata uint32_t ao_log_end_pos; -__pdata uint32_t ao_log_current_pos; - -__xdata union ao_log_single ao_log_single_write_data; -__xdata union ao_log_single ao_log_single_read_data; - -uint8_t -ao_log_single_write(void) -{ - uint8_t wrote = 0; - - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_single_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - &ao_log_single_write_data, - AO_LOG_SINGLE_SIZE); - ao_log_current_pos += AO_LOG_SINGLE_SIZE; - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_single_valid(void) -{ - __xdata uint8_t *d = ao_log_single_read_data.bytes; - uint8_t i; - for (i = 0; i < AO_LOG_SINGLE_SIZE; i++) - if (*d++ != 0xff) - return 1; - return 0; -} - -uint8_t -ao_log_single_read(uint32_t pos) -{ - if (!ao_storage_read(pos, &ao_log_single_read_data, AO_LOG_SINGLE_SIZE)) - return 0; - return ao_log_single_valid(); -} - -void -ao_log_single_start(void) -{ - if (!ao_log_running) { - ao_log_running = 1; - ao_wakeup(&ao_log_running); - } -} - -void -ao_log_single_stop(void) -{ - if (ao_log_running) { - ao_log_running = 0; - } -} - -void -ao_log_single_restart(void) -{ - /* Find end of data */ - ao_log_end_pos = ao_storage_config; - for (ao_log_current_pos = 0; - ao_log_current_pos < ao_storage_config; - ao_log_current_pos += ao_storage_block) - { - if (!ao_log_single_read(ao_log_current_pos)) - break; - } - if (ao_log_current_pos > 0) { - ao_log_current_pos -= ao_storage_block; - for (; ao_log_current_pos < ao_storage_config; - ao_log_current_pos += sizeof (struct ao_log_telescience)) - { - if (!ao_log_single_read(ao_log_current_pos)) - break; - } - } -} - -void -ao_log_single_set(void) -{ - printf("Logging currently %s\n", ao_log_running ? "on" : "off"); - ao_cmd_hex(); - if (ao_cmd_status == ao_cmd_success) { - if (ao_cmd_lex_i) { - printf("Logging from %ld to %ld\n", ao_log_current_pos, ao_log_end_pos); - ao_log_single_start(); - } else { - printf ("Log stopped at %ld\n", ao_log_current_pos); - ao_log_single_stop(); - } - } - ao_cmd_status = ao_cmd_success; -} - -void -ao_log_single_delete(void) -{ - uint32_t pos; - - ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) - return; - if (ao_cmd_lex_i != 1) { - ao_cmd_status = ao_cmd_syntax_error; - printf("No such flight: %d\n", ao_cmd_lex_i); - return; - } - ao_log_single_stop(); - for (pos = 0; pos < ao_storage_config; pos += ao_storage_block) { - if (!ao_log_single_read(pos)) - break; - ao_storage_erase(pos); - } - ao_log_current_pos = ao_log_start_pos = 0; - if (pos == 0) - printf("No such flight: %d\n", ao_cmd_lex_i); - else - printf ("Erased\n"); -} - -uint8_t -ao_log_full(void) -{ - return ao_log_current_pos >= ao_log_end_pos; -} - -uint8_t -ao_log_present(void) -{ - return ao_log_single_read(0); -} - -static void -ao_log_single_query(void) -{ - printf("Logging enabled: %d\n", ao_log_running); - printf("Log start: %ld\n", ao_log_start_pos); - printf("Log cur: %ld\n", ao_log_current_pos); - printf("Log end: %ld\n", ao_log_end_pos); - ao_log_single_extra_query(); -} - -const struct ao_cmds ao_log_single_cmds[] = { - { ao_log_single_set, "L <0 off, 1 on>\0Set logging" }, - { ao_log_single_list, "l\0List stored logs" }, - { ao_log_single_delete, "d 1\0Delete all stored logs" }, - { ao_log_single_query, "q\0Query log status" }, - { 0, NULL }, -}; - -void -ao_log_single_init(void) -{ - ao_log_running = 0; - - ao_cmd_register(&ao_log_single_cmds[0]); - - ao_add_task(&ao_log_single_task, ao_log_single, "log"); -} diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c deleted file mode 100644 index 095aca37..00000000 --- a/src/core/ao_log_telem.c +++ /dev/null @@ -1,131 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include -#include - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRY; - -static __data uint8_t ao_log_monitor_pos; -__pdata enum ao_flight_state ao_flight_state; -__xdata int16_t ao_max_height; /* max of ao_height */ -__pdata int16_t sense_d, sense_m; -__pdata uint8_t ao_igniter_present; - -static void -ao_log_telem_track() { - if (ao_monitoring == sizeof (union ao_telemetry_all)) { - switch (ao_log_single_write_data.telemetry.generic.type) { - case AO_TELEMETRY_SENSOR_TELEMETRUM: - case AO_TELEMETRY_SENSOR_TELEMINI: - /* fall through ... */ - case AO_TELEMETRY_SENSOR_TELENANO: - if (ao_log_single_write_data.telemetry.generic.type == AO_TELEMETRY_SENSOR_TELENANO) { - ao_igniter_present = 0; - } else { - sense_d = ao_log_single_write_data.telemetry.sensor.sense_d; - sense_m = ao_log_single_write_data.telemetry.sensor.sense_m; - ao_igniter_present = 1; - } - if (ao_log_single_write_data.telemetry.sensor.height > ao_max_height) { - ao_max_height = ao_log_single_write_data.telemetry.sensor.height; - } - if (ao_log_single_write_data.telemetry.sensor.state != ao_flight_state) { - ao_flight_state = ao_log_single_write_data.telemetry.sensor.state; - if (ao_flight_state == ao_flight_pad) - ao_max_height = 0; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - } - } -} - -enum ao_igniter_status -ao_igniter_status(enum ao_igniter igniter) -{ - int16_t value; - - switch (igniter) { - case ao_igniter_drogue: - value = sense_d; - break; - case ao_igniter_main: - value = sense_m; - break; - default: - value = 0; - break; - } - if (value < AO_IGNITER_OPEN) - return ao_igniter_open; - else if (value > AO_IGNITER_CLOSED) - return ao_igniter_ready; - else - return ao_igniter_unknown; -} - -void -ao_log_single(void) -{ - ao_storage_setup(); - - /* This can take a while, so let the rest - * of the system finish booting before we start - */ - ao_delay(AO_SEC_TO_TICKS(2)); - - ao_log_running = 1; - ao_log_single_restart(); - ao_flight_state = ao_flight_startup; - ao_monitor_set(sizeof(struct ao_telemetry_generic)); - - for (;;) { - while (!ao_log_running) - ao_sleep(&ao_log_running); - - ao_log_monitor_pos = ao_monitor_head; - while (ao_log_running) { - /* Write samples to EEPROM */ - while (ao_log_monitor_pos != ao_monitor_head) { - ao_xmemcpy(&ao_log_single_write_data.telemetry, - &ao_monitor_ring[ao_log_monitor_pos], - AO_LOG_SINGLE_SIZE); - ao_log_single_write(); - ao_log_monitor_pos = ao_monitor_ring_next(ao_log_monitor_pos); - ao_log_telem_track(); - } - /* Wait for more telemetry data to arrive */ - ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); - } - } -} - -void -ao_log_single_list(void) -{ - if (ao_log_current_pos != 0) - printf("flight 1 start %x end %x\n", - 0, - (uint16_t) ((ao_log_current_pos + 0xff) >> 8)); - printf ("done\n"); -} - -void -ao_log_single_extra_query(void) -{ -} diff --git a/src/core/ao_log_telescience.c b/src/core/ao_log_telescience.c deleted file mode 100644 index 002a10bd..00000000 --- a/src/core/ao_log_telescience.c +++ /dev/null @@ -1,119 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include "ao_product.h" -#include "ao_log.h" -#include "ao_companion.h" - -static uint8_t ao_log_data_pos; - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TELESCIENCE; - -static void -ao_log_telescience_csum(void) __reentrant -{ - __xdata uint8_t *b = ao_log_single_write_data.bytes; - uint8_t sum = 0x5a; - uint8_t i; - - ao_log_single_write_data.telescience.csum = 0; - for (i = 0; i < sizeof (struct ao_log_telescience); i++) - sum += *b++; - ao_log_single_write_data.telescience.csum = -sum; -} - -void -ao_log_single(void) -{ - ao_storage_setup(); - - /* This can take a while, so let the rest - * of the system finish booting before we start - */ - ao_delay(AO_SEC_TO_TICKS(10)); - - ao_log_single_restart(); - for (;;) { - while (!ao_log_running) - ao_sleep(&ao_log_running); - - ao_log_start_pos = ao_log_current_pos; - ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_START; - ao_log_single_write_data.telescience.tick = ao_time(); - ao_log_single_write_data.telescience.adc[0] = ao_companion_command.serial; - ao_log_single_write_data.telescience.adc[1] = ao_companion_command.flight; - ao_log_telescience_csum(); - ao_log_single_write(); - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_data_head); - ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_DATA; - while (ao_log_running) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_data_head) { - ao_log_single_write_data.telescience.tick = ao_data_ring[ao_log_data_pos].tick; - memcpy(&ao_log_single_write_data.telescience.adc, (void *) ao_data_ring[ao_log_data_pos].adc.adc, - AO_LOG_TELESCIENCE_NUM_ADC * sizeof (uint16_t)); - ao_log_telescience_csum(); - ao_log_single_write(); - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } - /* Wait for more ADC data to arrive */ - ao_sleep((void *) &ao_data_head); - } - memset(&ao_log_single_write_data.telescience.adc, '\0', sizeof (ao_log_single_write_data.telescience.adc)); - } -} - -void -ao_log_single_list(void) -{ - uint32_t pos; - uint32_t start = 0; - uint8_t flight = 0; - - for (pos = 0; ; pos += sizeof (struct ao_log_telescience)) { - if (pos >= ao_storage_config || - !ao_log_single_read(pos) || - ao_log_single_read_data.telescience.type == AO_LOG_TELESCIENCE_START) - { - if (pos != start) { - printf("flight %d start %x end %x\n", - flight, - (uint16_t) (start >> 8), - (uint16_t) ((pos + 0xff) >> 8)); flush(); - } - if (ao_log_single_read_data.telescience.type != AO_LOG_TELESCIENCE_START) - break; - start = pos; - flight++; - } - } - printf ("done\n"); -} - -void -ao_log_single_extra_query(void) -{ - printf("log data tick: %04x\n", ao_log_single_write_data.telescience.tick); - printf("TM data tick: %04x\n", ao_log_single_write_data.telescience.tm_tick); - printf("TM state: %d\n", ao_log_single_write_data.telescience.tm_state); - printf("TM serial: %d\n", ao_companion_command.serial); - printf("TM flight: %d\n", ao_companion_command.flight); -} diff --git a/src/core/ao_log_tiny.c b/src/core/ao_log_tiny.c deleted file mode 100644 index 67767dc9..00000000 --- a/src/core/ao_log_tiny.c +++ /dev/null @@ -1,161 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -static __data uint16_t ao_log_tiny_interval; - -#define AO_LOG_TINY_INTERVAL_DEFAULT AO_MS_TO_TICKS(1000) -#if USE_FAST_ASCENT_LOG -#define AO_LOG_TINY_INTERVAL_ASCENT AO_MS_TO_TICKS(100) -#define AO_PAD_RING 8 -#else -#define AO_LOG_TINY_INTERVAL_ASCENT AO_LOG_TINY_INTERVAL_DEFAULT -#define AO_PAD_RING 2 -#endif - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TINY; - -void -ao_log_tiny_set_interval(uint16_t ticks) -{ - ao_log_tiny_interval = ticks; -} - - -static void ao_log_tiny_data(uint16_t d) -{ - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - ao_storage_write(ao_log_current_pos, DATA_TO_XDATA(&d), 2); - ao_log_current_pos += 2; - } -} - -static __xdata uint16_t ao_log_pad_ring[AO_PAD_RING]; -static __pdata uint8_t ao_log_pad_ring_pos; - -#define ao_pad_ring_next(n) (((n) + 1) & (AO_PAD_RING - 1)) - -static void ao_log_tiny_queue(uint16_t d) -{ - ao_log_pad_ring[ao_log_pad_ring_pos] = d; - ao_log_pad_ring_pos = ao_pad_ring_next(ao_log_pad_ring_pos); -} - -static void ao_log_tiny_start(void) -{ - uint8_t p; - uint16_t d; - - ao_log_tiny_data(ao_flight_number); - ao_log_tiny_data(ao_ground_pres); - p = ao_log_pad_ring_pos; - do { - d = ao_log_pad_ring[p]; - /* - * ignore unwritten slots - */ - if (d) - ao_log_tiny_data(d); - p = ao_pad_ring_next(p); - } while (p != ao_log_pad_ring_pos); -} - -void -ao_log(void) -{ - uint16_t last_time; - uint16_t now; - enum ao_flight_state ao_log_tiny_state; - int32_t sum; - int16_t count; - uint8_t ao_log_data; - uint8_t ao_log_started = 0; - - ao_storage_setup(); - - ao_log_scan(); - - ao_log_tiny_state = ao_flight_invalid; - ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT; - sum = 0; - count = 0; - ao_log_data = ao_sample_data; - last_time = ao_time(); - for (;;) { - - /* - * Add in pending sample data - */ - ao_sleep(DATA_TO_XDATA(&ao_sample_data)); - while (ao_log_data != ao_sample_data) { - sum += ao_data_pres(&ao_data_ring[ao_log_data]); - count++; - ao_log_data = ao_data_ring_next(ao_log_data); - } - if (ao_log_running) { - if (!ao_log_started) { - ao_log_tiny_start(); - ao_log_started = 1; - } - if (ao_flight_state != ao_log_tiny_state) { - ao_log_tiny_data(ao_flight_state | 0x8000); - ao_log_tiny_state = ao_flight_state; - ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_DEFAULT; -#if AO_LOG_TINY_INTERVAL_ASCENT != AO_LOG_TINY_INTERVAL_DEFAULT - if (ao_log_tiny_state <= ao_flight_coast) - ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT; -#endif - if (ao_log_tiny_state == ao_flight_landed) - ao_log_stop(); - } - } - - /* Stop logging when told to */ - if (!ao_log_running && ao_log_started) - ao_exit(); - - /* - * Write out the sample when finished - */ - now = ao_time(); - if ((int16_t) (now - (last_time + ao_log_tiny_interval)) >= 0 && count) { - count = sum / count; - if (ao_log_started) - ao_log_tiny_data(count); - else - ao_log_tiny_queue(count); - sum = 0; - count = 0; - last_time = now; - } - } -} - -uint16_t -ao_log_flight(uint8_t slot) -{ - static __xdata uint16_t flight; - - (void) slot; - ao_storage_read(0, &flight, 2); - if (flight == 0xffff) - flight = 0; - return flight; -} diff --git a/src/core/ao_microflight.c b/src/core/ao_microflight.c deleted file mode 100644 index f680e400..00000000 --- a/src/core/ao_microflight.c +++ /dev/null @@ -1,143 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_FLIGHT_TEST -#include -#endif -#include -#include - -uint32_t pa; -uint32_t pa_ground; -uint32_t pa_min; - -static void -ao_microsample(void) -{ - ao_pa_get(); - ao_microkalman_predict(); - ao_microkalman_correct(); -} - -#define NUM_PA_HIST (GROUND_AVG) - -#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) - -static uint32_t pa_hist[NUM_PA_HIST]; - -void -ao_microflight(void) -{ - int16_t sample_count; - uint16_t time; - uint32_t pa_interval_min, pa_interval_max; - int32_t pa_diff; - uint8_t h, i; - uint8_t accel_lock = 0; - uint32_t pa_sum = 0; - - /* Wait for motion, averaging values to get ground pressure */ - - time = ao_time(); - ao_pa_get(); - ao_microkalman_init(); - pa_ground = pa; - sample_count = 0; - h = 0; - for (;;) { - time += SAMPLE_SLEEP; - if ((sample_count & 0x1f) == 0) - ao_led_on(AO_LED_REPORT); - ao_delay_until(time); - ao_microsample(); - if ((sample_count & 0x1f) == 0) - ao_led_off(AO_LED_REPORT); - pa_hist[h] = pa; - h = SKIP_PA_HIST(h,1); - pa_diff = pa_ground - ao_pa; - - /* Check for a significant pressure change */ - if (pa_diff > BOOST_DETECT) - break; - - if (sample_count < GROUND_AVG * 2) { - if (sample_count < GROUND_AVG) - pa_sum += pa; - ++sample_count; - } else { - pa_ground = pa_sum >> GROUND_AVG_SHIFT; - pa_sum = 0; - sample_count = 0; - } - } - - /* Go back and find the last sample close to the ground */ - pa_min = pa_ground - LAND_DETECT; - for (i = SKIP_PA_HIST(h,-2); i != SKIP_PA_HIST(h,2); i = SKIP_PA_HIST(i,-2)) { - if (pa_hist[i] >= pa_min) - break; - } - - /* Log the remaining samples so we get a complete history since leaving the ground */ - for (; i != h; i = SKIP_PA_HIST(i,2)) { - pa = pa_hist[i]; - ao_log_micro_data(); - } - - /* Now sit around until the pressure is stable again and record the max */ - - sample_count = 0; - pa_min = ao_pa; - pa_interval_min = ao_pa; - pa_interval_max = ao_pa; - for (;;) { - time += SAMPLE_SLEEP; - ao_delay_until(time); - if ((sample_count & 3) == 0) - ao_led_on(AO_LED_REPORT); - ao_microsample(); - if ((sample_count & 3) == 0) - ao_led_off(AO_LED_REPORT); - if (sample_count & 1) - ao_log_micro_data(); - - /* If accelerating upwards, don't look for min pressure */ - if (ao_pa_accel < ACCEL_LOCK_PA) - accel_lock = ACCEL_LOCK_TIME; - else if (accel_lock) - --accel_lock; - else if (ao_pa < pa_min) - pa_min = ao_pa; - - if (sample_count == (GROUND_AVG - 1)) { - pa_diff = pa_interval_max - pa_interval_min; - - /* Check to see if the pressure is now stable */ - if (pa_diff < LAND_DETECT) - break; - sample_count = 0; - pa_interval_min = ao_pa; - pa_interval_max = ao_pa; - } else { - if (ao_pa < pa_interval_min) - pa_interval_min = ao_pa; - if (ao_pa > pa_interval_max) - pa_interval_max = ao_pa; - ++sample_count; - } - } -} diff --git a/src/core/ao_microkalman.c b/src/core/ao_microkalman.c deleted file mode 100644 index 0684ea2b..00000000 --- a/src/core/ao_microkalman.c +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_FLIGHT_TEST -#include -#endif -#include - -#define FIX_BITS 16 - -#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) -#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) -#define from_fix8(x) ((x) >> 8) -#define from_fix(x) ((x) >> 16) -#define fix8_to_fix16(x) ((x) << 8) -#define fix16_to_fix8(x) ((x) >> 8) - -#include - -/* Basic time step (96ms) */ -#define AO_MK_STEP to_fix16(0.096) -/* step ** 2 / 2 */ -#define AO_MK_STEP_2_2 to_fix16(0.004608) - -uint32_t ao_k_pa; /* 24.8 fixed point */ -int32_t ao_k_pa_speed; /* 16.16 fixed point */ -int32_t ao_k_pa_accel; /* 16.16 fixed point */ - -uint32_t ao_pa; /* integer portion */ -int16_t ao_pa_speed; /* integer portion */ -int16_t ao_pa_accel; /* integer portion */ - -void -ao_microkalman_init(void) -{ - ao_pa = pa; - ao_k_pa = pa << 8; -} - -void -ao_microkalman_predict(void) -{ - ao_k_pa += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2); - ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP; -} - -void -ao_microkalman_correct(void) -{ - int16_t e; /* Height error in Pa */ - - e = pa - from_fix8(ao_k_pa); - - ao_k_pa += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10); - ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10; - ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10; - ao_pa = from_fix8(ao_k_pa); - ao_pa_speed = from_fix(ao_k_pa_speed); - ao_pa_accel = from_fix(ao_k_pa_accel); -} diff --git a/src/core/ao_monitor.c b/src/core/ao_monitor.c deleted file mode 100644 index 18f170b4..00000000 --- a/src/core/ao_monitor.c +++ /dev/null @@ -1,308 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include "ao_telem.h" -#include "ao_flight.h" - -#if !HAS_MONITOR -#error Must define HAS_MONITOR to 1 -#endif - -#ifndef LEGACY_MONITOR -#error Must define LEGACY_MONITOR -#endif - -#ifndef HAS_MONITOR_PUT -#define HAS_MONITOR_PUT 1 -#endif - -#ifndef AO_MONITOR_LED -#error Must define AO_MONITOR_LED -#endif - -__data uint8_t ao_monitoring; -static __data uint8_t ao_monitor_disabled; -static __data uint8_t ao_internal_monitoring; -static __data uint8_t ao_external_monitoring; - -__xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; - -__data uint8_t ao_monitor_head; - -static void -_ao_monitor_adjust(void) -{ - if (ao_monitoring) - ao_radio_recv_abort(); - if (ao_monitor_disabled) - ao_monitoring = 0; - else { - if (ao_external_monitoring) - ao_monitoring = ao_external_monitoring; - else - ao_monitoring = ao_internal_monitoring; - } - ao_wakeup(DATA_TO_XDATA(&ao_monitoring)); -} - -void -ao_monitor_get(void) -{ - uint8_t size; - - for (;;) { - switch (ao_monitoring) { - case 0: - ao_sleep(DATA_TO_XDATA(&ao_monitoring)); - continue; -#if LEGACY_MONITOR - case AO_MONITORING_ORIG: - size = sizeof (struct ao_telemetry_orig_recv); - break; -#endif - default: - if (ao_monitoring > AO_MAX_TELEMETRY) - ao_monitoring = AO_MAX_TELEMETRY; - size = ao_monitoring; - break; - } - if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2, 0)) - continue; - ao_monitor_head = ao_monitor_ring_next(ao_monitor_head); - ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); - } -} - -#if AO_MONITOR_LED -__xdata struct ao_task ao_monitor_blink_task; - -void -ao_monitor_blink(void) -{ - for (;;) { - ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); - ao_led_for(AO_MONITOR_LED, AO_MS_TO_TICKS(100)); - } -} -#endif - -#if HAS_MONITOR_PUT - -static const char xdigit[16] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' -}; - -#define hex(c) do { putchar(xdigit[(c) >> 4]); putchar(xdigit[(c)&0xf]); } while (0) - -void -ao_monitor_put(void) -{ -#if LEGACY_MONITOR - __xdata char callsign[AO_MAX_CALLSIGN+1]; - int16_t rssi; -#endif - uint8_t ao_monitor_tail; - uint8_t state; - uint8_t sum, byte; - __xdata union ao_monitor *m; - -#define recv_raw ((m->raw)) -#define recv_orig ((m->orig)) -#define recv_tiny ((m->tiny)) - - ao_monitor_tail = ao_monitor_head; - for (;;) { - while (!ao_external_monitoring) - ao_sleep(DATA_TO_XDATA(&ao_external_monitoring)); - while (ao_monitor_tail == ao_monitor_head && ao_external_monitoring) - ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); - if (!ao_external_monitoring) - continue; - m = &ao_monitor_ring[ao_monitor_tail]; - ao_monitor_tail = ao_monitor_ring_next(ao_monitor_tail); - switch (ao_monitoring) { - case 0: - break; -#if LEGACY_MONITOR - case AO_MONITORING_ORIG: - state = recv_orig.telemetry_orig.flight_state; - - rssi = (int16_t) AO_RSSI_FROM_RADIO(recv_orig.rssi); - ao_xmemcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN); - if (state > ao_flight_invalid) - state = ao_flight_invalid; - if (recv_orig.status & PKT_APPEND_STATUS_1_CRC_OK) { - - /* General header fields */ - printf(AO_TELEM_VERSION " %d " - AO_TELEM_CALL " %s " - AO_TELEM_SERIAL " %d " - AO_TELEM_FLIGHT " %d " - AO_TELEM_RSSI " %d " - AO_TELEM_STATE " %s " - AO_TELEM_TICK " %d ", - AO_TELEMETRY_VERSION, - callsign, - recv_orig.telemetry_orig.serial, - recv_orig.telemetry_orig.flight, - rssi, - ao_state_names[state], - recv_orig.telemetry_orig.adc.tick); - - /* Raw sensor values */ - printf(AO_TELEM_RAW_ACCEL " %d " - AO_TELEM_RAW_BARO " %d " - AO_TELEM_RAW_THERMO " %d " - AO_TELEM_RAW_BATT " %d " - AO_TELEM_RAW_DROGUE " %d " - AO_TELEM_RAW_MAIN " %d ", - recv_orig.telemetry_orig.adc.accel, - recv_orig.telemetry_orig.adc.pres, - recv_orig.telemetry_orig.adc.temp, - recv_orig.telemetry_orig.adc.v_batt, - recv_orig.telemetry_orig.adc.sense_d, - recv_orig.telemetry_orig.adc.sense_m); - - /* Sensor calibration values */ - printf(AO_TELEM_CAL_ACCEL_GROUND " %d " - AO_TELEM_CAL_BARO_GROUND " %d " - AO_TELEM_CAL_ACCEL_PLUS " %d " - AO_TELEM_CAL_ACCEL_MINUS " %d ", - recv_orig.telemetry_orig.ground_accel, - recv_orig.telemetry_orig.ground_pres, - recv_orig.telemetry_orig.accel_plus_g, - recv_orig.telemetry_orig.accel_minus_g); - - if (recv_orig.telemetry_orig.u.k.unused == 0x8000) { - /* Kalman state values */ - printf(AO_TELEM_KALMAN_HEIGHT " %d " - AO_TELEM_KALMAN_SPEED " %d " - AO_TELEM_KALMAN_ACCEL " %d ", - recv_orig.telemetry_orig.height, - recv_orig.telemetry_orig.u.k.speed, - recv_orig.telemetry_orig.accel); - } else { - /* Ad-hoc flight values */ - printf(AO_TELEM_ADHOC_ACCEL " %d " - AO_TELEM_ADHOC_SPEED " %ld " - AO_TELEM_ADHOC_BARO " %d ", - recv_orig.telemetry_orig.accel, - recv_orig.telemetry_orig.u.flight_vel, - recv_orig.telemetry_orig.height); - } - ao_gps_print(&recv_orig.telemetry_orig.gps); - ao_gps_tracking_print(&recv_orig.telemetry_orig.gps_tracking); - putchar('\n'); -#if HAS_RSSI - ao_rssi_set(rssi); -#endif - } else { - printf("CRC INVALID RSSI %3d\n", rssi); - } - break; -#endif /* LEGACY_MONITOR */ - default: -#if AO_PROFILE - { - extern uint32_t ao_rx_start_tick, ao_rx_packet_tick, ao_rx_done_tick, ao_rx_last_done_tick; - extern uint32_t ao_fec_decode_start, ao_fec_decode_end; - - printf ("between packet: %d\n", ao_rx_start_tick - ao_rx_last_done_tick); - printf ("receive start delay: %d\n", ao_rx_packet_tick - ao_rx_start_tick); - printf ("decode time: %d\n", ao_fec_decode_end - ao_fec_decode_start); - printf ("rx cleanup: %d\n", ao_rx_done_tick - ao_fec_decode_end); - } -#endif - printf("TELEM "); - hex((uint8_t) (ao_monitoring + 2)); - sum = 0x5a; - for (state = 0; state < ao_monitoring + 2; state++) { - byte = recv_raw.packet[state]; - sum += byte; - hex(byte); - } - hex(sum); - putchar ('\n'); -#if HAS_RSSI - if (recv_raw.packet[ao_monitoring + 1] & PKT_APPEND_STATUS_1_CRC_OK) { - rssi = AO_RSSI_FROM_RADIO(recv_raw.packet[ao_monitoring]); - ao_rssi_set(rssi); - } -#endif - break; - } - ao_usb_flush(); - } -} - -__xdata struct ao_task ao_monitor_put_task; -#endif - -__xdata struct ao_task ao_monitor_get_task; - -void -ao_monitor_set(uint8_t monitoring) -{ - ao_internal_monitoring = monitoring; - _ao_monitor_adjust(); -} - -void -ao_monitor_disable(void) -{ - ++ao_monitor_disabled; - _ao_monitor_adjust(); -} - -void -ao_monitor_enable(void) -{ - --ao_monitor_disabled; - _ao_monitor_adjust(); -} - -#if HAS_MONITOR_PUT -static void -set_monitor(void) -{ - ao_cmd_hex(); - ao_external_monitoring = ao_cmd_lex_i; - ao_wakeup(DATA_TO_XDATA(&ao_external_monitoring)); - ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); - _ao_monitor_adjust(); -} - -__code struct ao_cmds ao_monitor_cmds[] = { - { set_monitor, "m <0 off, 1 old, 20 std>\0Set radio monitoring" }, - { 0, NULL }, -}; -#endif - -void -ao_monitor_init(void) __reentrant -{ -#if HAS_MONITOR_PUT - ao_cmd_register(&ao_monitor_cmds[0]); - ao_add_task(&ao_monitor_put_task, ao_monitor_put, "monitor_put"); -#endif - ao_add_task(&ao_monitor_get_task, ao_monitor_get, "monitor_get"); -#if AO_MONITOR_LED - ao_add_task(&ao_monitor_blink_task, ao_monitor_blink, "monitor_blink"); -#endif -} diff --git a/src/core/ao_mutex.c b/src/core/ao_mutex.c deleted file mode 100644 index 952ff462..00000000 --- a/src/core/ao_mutex.c +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -void -ao_mutex_get(__xdata uint8_t *mutex) __reentrant -{ - if (*mutex == ao_cur_task->task_id) - ao_panic(AO_PANIC_MUTEX); - ao_arch_critical( - while (*mutex) - ao_sleep(mutex); - *mutex = ao_cur_task->task_id; - ); -} - -void -ao_mutex_put(__xdata uint8_t *mutex) __reentrant -{ - if (*mutex != ao_cur_task->task_id) - ao_panic(AO_PANIC_MUTEX); - ao_arch_critical( - *mutex = 0; - ao_wakeup(mutex); - ); -} diff --git a/src/core/ao_notask.c b/src/core/ao_notask.c deleted file mode 100644 index 6f967e6d..00000000 --- a/src/core/ao_notask.c +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include - -static volatile void *ao_wchan; - -uint8_t -ao_sleep(__xdata void *wchan) -{ -#if 1 - ao_wchan = wchan; - ao_arch_wait_interrupt(); -#else - uint8_t sreg; - - ao_wchan = wchan; - asm("in %0,__SREG__" : "=&r" (sreg)); - sei(); - while (ao_wchan) - ao_arch_cpu_idle(); - asm("out __SREG__,%0" : : "r" (sreg)); -#endif - return 0; -} - -void -ao_wakeup(__xdata void *wchan) -{ - (void) wchan; - ao_wchan = 0; -} diff --git a/src/core/ao_notask.h b/src/core/ao_notask.h deleted file mode 100644 index 6b6b5bb8..00000000 --- a/src/core/ao_notask.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_NOTASK_H_ -#define _AO_NOTASK_H_ - -uint8_t -ao_sleep(__xdata void *wchan); - -void -ao_wakeup(__xdata void *wchan); - -#endif /* _AO_NOTASK_H_ */ diff --git a/src/core/ao_packet.h b/src/core/ao_packet.h deleted file mode 100644 index b8426cf9..00000000 --- a/src/core/ao_packet.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_PACKET_H_ -#define _AO_PACKET_H_ - -/* - * ao_packet.c - * - * Packet-based command interface - */ - -#define AO_PACKET_MAX 64 -#define AO_PACKET_SYN (uint8_t) 0xff - -struct ao_packet { - uint8_t addr; - uint8_t len; - uint8_t seq; - uint8_t ack; - uint8_t d[AO_PACKET_MAX]; - uint8_t callsign[AO_MAX_CALLSIGN]; -}; - -struct ao_packet_recv { - struct ao_packet packet; - int8_t rssi; - uint8_t status; -}; - -extern __xdata struct ao_packet_recv ao_rx_packet; -extern __xdata struct ao_packet ao_tx_packet; -extern __xdata struct ao_task ao_packet_task; -extern __xdata uint8_t ao_packet_enable; -extern __xdata uint8_t ao_packet_master_sleeping; -extern __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used; -extern __xdata uint8_t ao_packet_restart; - -void -ao_packet_send(void); - -uint8_t -ao_packet_recv(void); - -void -ao_packet_flush(void); - -void -ao_packet_putchar(char c) __reentrant; - -int -_ao_packet_pollchar(void); - -#if PACKET_HAS_MASTER -/* ao_packet_master.c */ - -extern __xdata int8_t ao_packet_last_rssi; - -void -ao_packet_master_init(void); -#endif - -#if PACKET_HAS_SLAVE -/* ao_packet_slave.c */ - -void -ao_packet_slave_start(void); - -void -ao_packet_slave_stop(void); - -void -ao_packet_slave_init(uint8_t enable); - -#endif - -#endif /* _AO_PACKET_H_ */ diff --git a/src/core/ao_panic.c b/src/core/ao_panic.c deleted file mode 100644 index c29cd8fe..00000000 --- a/src/core/ao_panic.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -#ifndef HAS_BEEP -#error Please define HAS_BEEP -#endif - -#if !HAS_BEEP -#define ao_beep(x) -#endif -#if !LEDS_AVAILABLE -#define ao_led_on(x) -#define ao_led_off(x) -#endif - -#ifndef AO_LED_PANIC -#define AO_LED_PANIC AO_LED_RED -#endif - -static void -ao_panic_delay(uint8_t n) -{ - uint8_t i = 0, j = 0; - - while (n--) - while (--j) - while (--i) - ao_arch_nop(); -} - -void -ao_panic(uint8_t reason) -{ - uint8_t n; - -#if LOW_LEVEL_DEBUG - ao_cur_task = NULL; - printf ("panic %d\n", reason); -#endif - ao_arch_block_interrupts(); - for (;;) { - ao_panic_delay(20); - for (n = 0; n < 5; n++) { - ao_led_on(AO_LED_PANIC); - ao_beep(AO_BEEP_HIGH); - ao_panic_delay(1); - ao_led_off(AO_LED_PANIC); - ao_beep(AO_BEEP_LOW); - ao_panic_delay(1); - } - ao_beep(AO_BEEP_OFF); - ao_panic_delay(2); - -#ifdef SDCC -#pragma disable_warning 126 -#endif - if (reason & 0x40) { - ao_led_on(AO_LED_PANIC); - ao_beep(AO_BEEP_HIGH); - ao_panic_delay(40); - ao_led_off(AO_LED_PANIC); - ao_beep(AO_BEEP_OFF); - ao_panic_delay(10); - } - for (n = 0; n < (reason & 0x3f); n++) { - ao_led_on(AO_LED_PANIC); - ao_beep(AO_BEEP_MID); - ao_panic_delay(10); - ao_led_off(AO_LED_PANIC); - ao_beep(AO_BEEP_OFF); - ao_panic_delay(10); - } - } -} diff --git a/src/core/ao_product.c b/src/core/ao_product.c deleted file mode 100644 index b9327bac..00000000 --- a/src/core/ao_product.c +++ /dev/null @@ -1,161 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include "ao_product.h" - -/* Defines which mark this particular AltOS product */ - -const char ao_version[AO_MAX_VERSION] = AO_iVersion_STRING; -const char ao_manufacturer[] = AO_iManufacturer_STRING; -const char ao_product[] = AO_iProduct_STRING; - -#define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) - -#if HAS_USB - -/* Maximum power in mA */ -#ifndef AO_USB_MAX_POWER -#define AO_USB_MAX_POWER 100 -#endif - -#include "ao_usb.h" -/* USB descriptors in one giant block of bytes */ -AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = -{ - /* Device descriptor */ - 0x12, - AO_USB_DESC_DEVICE, - LE_WORD(0x0110), /* bcdUSB */ - 0x02, /* bDeviceClass */ - 0x00, /* bDeviceSubClass */ - 0x00, /* bDeviceProtocol */ - AO_USB_CONTROL_SIZE, /* bMaxPacketSize */ - LE_WORD(0xFFFE), /* idVendor */ - LE_WORD(AO_idProduct_NUMBER), /* idProduct */ - LE_WORD(0x0100), /* bcdDevice */ - 0x01, /* iManufacturer */ - 0x02, /* iProduct */ - 0x03, /* iSerialNumber */ - 0x01, /* bNumConfigurations */ - - /* Configuration descriptor */ - 0x09, - AO_USB_DESC_CONFIGURATION, - LE_WORD(67), /* wTotalLength */ - 0x02, /* bNumInterfaces */ - 0x01, /* bConfigurationValue */ - 0x00, /* iConfiguration */ - 0xC0, /* bmAttributes */ - AO_USB_MAX_POWER >> 1, /* bMaxPower, 2mA units */ - - /* Control class interface */ - 0x09, - AO_USB_DESC_INTERFACE, - 0x00, /* bInterfaceNumber */ - 0x00, /* bAlternateSetting */ - 0x01, /* bNumEndPoints */ - 0x02, /* bInterfaceClass */ - 0x02, /* bInterfaceSubClass */ - 0x01, /* bInterfaceProtocol, linux requires value of 1 for the cdc_acm module */ - 0x00, /* iInterface */ - - /* Header functional descriptor */ - 0x05, - AO_USB_CS_INTERFACE, - 0x00, /* bDescriptor SubType Header */ - LE_WORD(0x0110), /* CDC version 1.1 */ - - /* Call management functional descriptor */ - 0x05, - AO_USB_CS_INTERFACE, - 0x01, /* bDescriptor SubType Call Management */ - 0x01, /* bmCapabilities = device handles call management */ - 0x01, /* bDataInterface call management interface number */ - - /* ACM functional descriptor */ - 0x04, - AO_USB_CS_INTERFACE, - 0x02, /* bDescriptor SubType Abstract Control Management */ - 0x02, /* bmCapabilities = D1 (Set_line_Coding, Set_Control_Line_State, Get_Line_Coding and Serial_State) */ - - /* Union functional descriptor */ - 0x05, - AO_USB_CS_INTERFACE, - 0x06, /* bDescriptor SubType Union Functional descriptor */ - 0x00, /* bMasterInterface */ - 0x01, /* bSlaveInterface0 */ - - /* Notification EP */ - 0x07, - AO_USB_DESC_ENDPOINT, - AO_USB_INT_EP|0x80, /* bEndpointAddress */ - 0x03, /* bmAttributes = intr */ - LE_WORD(8), /* wMaxPacketSize */ - 0xff, /* bInterval */ - - /* Data class interface descriptor */ - 0x09, - AO_USB_DESC_INTERFACE, - 0x01, /* bInterfaceNumber */ - 0x00, /* bAlternateSetting */ - 0x02, /* bNumEndPoints */ - 0x0A, /* bInterfaceClass = data */ - 0x00, /* bInterfaceSubClass */ - 0x00, /* bInterfaceProtocol */ - 0x00, /* iInterface */ - - /* Data EP OUT */ - 0x07, - AO_USB_DESC_ENDPOINT, - AO_USB_OUT_EP, /* bEndpointAddress */ - 0x02, /* bmAttributes = bulk */ - LE_WORD(AO_USB_OUT_SIZE),/* wMaxPacketSize */ - 0x00, /* bInterval */ - - /* Data EP in */ - 0x07, - AO_USB_DESC_ENDPOINT, - AO_USB_IN_EP|0x80, /* bEndpointAddress */ - 0x02, /* bmAttributes = bulk */ - LE_WORD(AO_USB_IN_SIZE),/* wMaxPacketSize */ - 0x00, /* bInterval */ - - /* String descriptors */ - 0x04, - AO_USB_DESC_STRING, - LE_WORD(0x0409), - - /* iManufacturer */ - AO_iManufacturer_LEN, - AO_USB_DESC_STRING, - AO_iManufacturer_UCS2, - - /* iProduct */ - AO_iProduct_LEN, - AO_USB_DESC_STRING, - AO_iProduct_UCS2, - - /* iSerial */ - AO_iSerial_LEN, - AO_USB_DESC_STRING, - AO_iSerial_UCS2, - - /* Terminating zero */ - 0 -}; -#endif diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c deleted file mode 100644 index e59f5bc4..00000000 --- a/src/core/ao_pyro.c +++ /dev/null @@ -1,518 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_FLIGHT_TEST -#include -#include -#include -#endif -#include - -#if IS_COMPANION -#include -#define ao_accel ao_companion_command.accel -#define ao_speed ao_companion_command.speed -#define ao_height ao_companion_command.height -#define ao_flight_state ao_companion_command.flight_state -#define ao_motor_number ao_companion_command.motor_number -#endif - -#define ao_lowbit(x) ((x) & (-x)) - -#ifndef AO_FLIGHT_TEST -enum ao_igniter_status -ao_pyro_status(uint8_t p) -{ - __xdata struct ao_data packet; - __pdata int16_t value; - - ao_arch_critical( - ao_data_get(&packet); - ); - - value = (AO_IGNITER_CLOSED>>1); - value = AO_SENSE_PYRO(&packet, p); - if (value < AO_IGNITER_OPEN) - return ao_igniter_open; - else if (value > AO_IGNITER_CLOSED) - return ao_igniter_ready; - else - return ao_igniter_unknown; -} - -void -ao_pyro_print_status(void) -{ - uint8_t p; - - for(p = 0; p < AO_PYRO_NUM; p++) { - enum ao_igniter_status status = ao_pyro_status(p); - printf("Igniter: %d Status: %s\n", - p, ao_igniter_status_names[status]); - } -} -#endif - -uint16_t ao_pyro_fired; - -/* - * Given a pyro structure, figure out - * if the current flight state satisfies all - * of the requirements - */ -static uint8_t -ao_pyro_ready(struct ao_pyro *pyro) -{ - enum ao_pyro_flag flag, flags; - - flags = pyro->flags; - while (flags != ao_pyro_none) { - flag = ao_lowbit(flags); - flags &= ~flag; - switch (flag) { - - case ao_pyro_accel_less: - if (ao_accel <= pyro->accel_less) - continue; - break; - case ao_pyro_accel_greater: - if (ao_accel >= pyro->accel_greater) - continue; - break; - - - case ao_pyro_speed_less: - if (ao_speed <= pyro->speed_less) - continue; - break; - case ao_pyro_speed_greater: - if (ao_speed >= pyro->speed_greater) - continue; - break; - - case ao_pyro_height_less: - if (ao_height <= pyro->height_less) - continue; - break; - case ao_pyro_height_greater: - if (ao_height >= pyro->height_greater) - continue; - break; - -#if HAS_GYRO - case ao_pyro_orient_less: - if (ao_sample_orient <= pyro->orient_less) - continue; - break; - case ao_pyro_orient_greater: - if (ao_sample_orient >= pyro->orient_greater) - continue; - break; -#endif - - case ao_pyro_time_less: - if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) - continue; - break; - case ao_pyro_time_greater: - if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) - continue; - break; - - case ao_pyro_ascending: - if (ao_speed > 0) - continue; - break; - case ao_pyro_descending: - if (ao_speed < 0) - continue; - break; - - case ao_pyro_after_motor: - if (ao_motor_number == pyro->motor) - continue; - break; - - case ao_pyro_delay: - /* handled separately */ - continue; - - case ao_pyro_state_less: - if (ao_flight_state < pyro->state_less) - continue; - break; - case ao_pyro_state_greater_or_equal: - if (ao_flight_state >= pyro->state_greater_or_equal) - continue; - break; - - default: - continue; - } - return FALSE; - } - return TRUE; -} - -#ifndef AO_FLIGHT_TEST -static void -ao_pyro_pin_set(uint8_t p, uint8_t v) -{ - switch (p) { -#if AO_PYRO_NUM > 0 - case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break; -#endif -#if AO_PYRO_NUM > 1 - case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break; -#endif -#if AO_PYRO_NUM > 2 - case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break; -#endif -#if AO_PYRO_NUM > 3 - case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break; -#endif -#if AO_PYRO_NUM > 4 - case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break; -#endif -#if AO_PYRO_NUM > 5 - case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break; -#endif -#if AO_PYRO_NUM > 6 - case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break; -#endif -#if AO_PYRO_NUM > 7 - case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; -#endif - default: break; - } -} -#endif - -uint8_t ao_pyro_wakeup; - -static void -ao_pyro_pins_fire(uint16_t fire) -{ - uint8_t p; - - for (p = 0; p < AO_PYRO_NUM; p++) { - if (fire & (1 << p)) - ao_pyro_pin_set(p, 1); - } - ao_delay(AO_MS_TO_TICKS(50)); - for (p = 0; p < AO_PYRO_NUM; p++) { - if (fire & (1 << p)) { - ao_pyro_pin_set(p, 0); - ao_config.pyro[p].fired = 1; - ao_pyro_fired |= (1 << p); - } - } - ao_delay(AO_MS_TO_TICKS(50)); -} - -static uint8_t -ao_pyro_check(void) -{ - struct ao_pyro *pyro; - uint8_t p, any_waiting; - uint16_t fire = 0; - - any_waiting = 0; - for (p = 0; p < AO_PYRO_NUM; p++) { - pyro = &ao_config.pyro[p]; - - /* Ignore igniters which have already fired - */ - if (pyro->fired) - continue; - - /* Ignore disabled igniters - */ - if (!pyro->flags) - continue; - - any_waiting = 1; - /* Check pyro state to see if it should fire - */ - if (!pyro->delay_done) { - if (!ao_pyro_ready(pyro)) - continue; - - /* If there's a delay set, then remember when - * it expires - */ - if (pyro->flags & ao_pyro_delay) { - pyro->delay_done = ao_time() + pyro->delay; - if (!pyro->delay_done) - pyro->delay_done = 1; - } - } - - /* Check to see if we're just waiting for - * the delay to expire - */ - if (pyro->delay_done) { - if ((int16_t) (ao_time() - pyro->delay_done) < 0) - continue; - } - - fire |= (1 << p); - } - - if (fire) - ao_pyro_pins_fire(fire); - - return any_waiting; -} - -#define NO_VALUE 0xff - -#define AO_PYRO_NAME_LEN 3 - -#if !DISABLE_HELP -#define ENABLE_HELP 1 -#endif - -#if ENABLE_HELP -#define HELP(s) (s) -#else -#define HELP(s) -#endif - -const struct { - char name[AO_PYRO_NAME_LEN]; - enum ao_pyro_flag flag; - uint8_t offset; -#if ENABLE_HELP - char *help; -#endif -} ao_pyro_values[] = { - { "a<", ao_pyro_accel_less, offsetof(struct ao_pyro, accel_less), HELP("accel less (m/ss * 16)") }, - { "a>", ao_pyro_accel_greater, offsetof(struct ao_pyro, accel_greater), HELP("accel greater (m/ss * 16)") }, - - { "s<", ao_pyro_speed_less, offsetof(struct ao_pyro, speed_less), HELP("speed less (m/s * 16)") }, - { "s>", ao_pyro_speed_greater, offsetof(struct ao_pyro, speed_greater), HELP("speed greater (m/s * 16)") }, - - { "h<", ao_pyro_height_less, offsetof(struct ao_pyro, height_less), HELP("height less (m)") }, - { "h>", ao_pyro_height_greater, offsetof(struct ao_pyro, height_greater), HELP("height greater (m)") }, - -#if HAS_GYRO - { "o<", ao_pyro_orient_less, offsetof(struct ao_pyro, orient_less), HELP("orient less (deg)") }, - { "o>", ao_pyro_orient_greater, offsetof(struct ao_pyro, orient_greater), HELP("orient greater (deg)") }, -#endif - - { "t<", ao_pyro_time_less, offsetof(struct ao_pyro, time_less), HELP("time less (s * 100)") }, - { "t>", ao_pyro_time_greater, offsetof(struct ao_pyro, time_greater), HELP("time greater (s * 100)") }, - - { "f<", ao_pyro_state_less, offsetof(struct ao_pyro, state_less), HELP("state less") }, - { "f>=",ao_pyro_state_greater_or_equal, offsetof(struct ao_pyro, state_greater_or_equal), HELP("state greater or equal") }, - - { "A", ao_pyro_ascending, NO_VALUE, HELP("ascending") }, - { "D", ao_pyro_descending, NO_VALUE, HELP("descending") }, - - { "m", ao_pyro_after_motor, offsetof(struct ao_pyro, motor), HELP("after motor") }, - - { "d", ao_pyro_delay, offsetof(struct ao_pyro, delay), HELP("delay before firing (s * 100)") }, - { "", ao_pyro_none, NO_VALUE, HELP(NULL) }, -}; - -#define NUM_PYRO_VALUES (sizeof ao_pyro_values / sizeof ao_pyro_values[0]) - -#ifndef AO_FLIGHT_TEST -static void -ao_pyro(void) -{ - uint8_t any_waiting; - - ao_config_get(); - while (ao_flight_state < ao_flight_boost) - ao_sleep(&ao_flight_state); - - for (;;) { - ao_alarm(AO_MS_TO_TICKS(100)); - ao_sleep(&ao_pyro_wakeup); - ao_clear_alarm(); - if (ao_flight_state >= ao_flight_landed) - break; - any_waiting = ao_pyro_check(); - if (!any_waiting) - break; - } - ao_exit(); -} - -__xdata struct ao_task ao_pyro_task; - - -static void -ao_pyro_print_name(uint8_t v) -{ - const char *s = ao_pyro_values[v].name; - printf ("%s%s", s, " " + strlen(s)); -} - -#if ENABLE_HELP -static void -ao_pyro_help(void) -{ - uint8_t v; - for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { - ao_pyro_print_name(v); - if (ao_pyro_values[v].offset != NO_VALUE) - printf (" "); - else - printf (" "); - printf ("%s\n", ao_pyro_values[v].help); - } -} -#endif - -void -ao_pyro_show(void) -{ - uint8_t p; - uint8_t v; - struct ao_pyro *pyro; - - printf ("Pyro-count: %d\n", AO_PYRO_NUM); - for (p = 0; p < AO_PYRO_NUM; p++) { - printf ("Pyro %2d: ", p); - pyro = &ao_config.pyro[p]; - if (!pyro->flags) { - printf ("\n"); - continue; - } - for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { - if (!(pyro->flags & ao_pyro_values[v].flag)) - continue; - ao_pyro_print_name(v); - if (ao_pyro_values[v].offset != NO_VALUE) { - int16_t value; - - value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); - printf ("%6d ", value); - } else { - printf (" "); - } - } - printf ("\n"); - } -} - -void -ao_pyro_set(void) -{ - uint8_t p; - struct ao_pyro pyro_tmp; - char name[AO_PYRO_NAME_LEN]; - uint8_t c; - uint8_t v; - - ao_cmd_white(); - -#if ENABLE_HELP - switch (ao_cmd_lex_c) { - case '?': - ao_pyro_help(); - return; - } -#endif - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - p = ao_cmd_lex_i; - if (AO_PYRO_NUM <= p) { - printf ("invalid pyro channel %d\n", p); - return; - } - pyro_tmp.flags = 0; - for (;;) { - ao_cmd_white(); - if (ao_cmd_lex_c == '\n') - break; - - for (c = 0; c < AO_PYRO_NAME_LEN - 1; c++) { - if (ao_cmd_is_white()) - break; - name[c] = ao_cmd_lex_c; - ao_cmd_lex(); - } - name[c] = '\0'; - for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { - if (!strcmp (ao_pyro_values[v].name, name)) - break; - } - if (ao_pyro_values[v].flag == ao_pyro_none) { - printf ("invalid pyro field %s\n", name); - ao_cmd_status = ao_cmd_syntax_error; - return; - } - pyro_tmp.flags |= ao_pyro_values[v].flag; - if (ao_pyro_values[v].offset != NO_VALUE) { - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; - } - } - _ao_config_edit_start(); - ao_config.pyro[p] = pyro_tmp; - _ao_config_edit_finish(); -} - -void -ao_pyro_manual(uint8_t p) -{ - printf ("ao_pyro_manual %d\n", p); - if (p >= AO_PYRO_NUM) { - ao_cmd_status = ao_cmd_syntax_error; - return; - } - ao_pyro_pins_fire(1 << p); -} - -void -ao_pyro_init(void) -{ -#if AO_PYRO_NUM > 0 - ao_enable_output(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, 0); -#endif -#if AO_PYRO_NUM > 1 - ao_enable_output(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, 0); -#endif -#if AO_PYRO_NUM > 2 - ao_enable_output(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, 0); -#endif -#if AO_PYRO_NUM > 3 - ao_enable_output(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, 0); -#endif -#if AO_PYRO_NUM > 4 - ao_enable_output(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, 0); -#endif -#if AO_PYRO_NUM > 5 - ao_enable_output(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, 0); -#endif -#if AO_PYRO_NUM > 6 - ao_enable_output(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, 0); -#endif -#if AO_PYRO_NUM > 7 - ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); -#endif - ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); -} -#endif diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h deleted file mode 100644 index 0c5642d6..00000000 --- a/src/core/ao_pyro.h +++ /dev/null @@ -1,83 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_PYRO_H_ -#define _AO_PYRO_H_ - -enum ao_pyro_flag { - ao_pyro_none = 0x00000000, - - ao_pyro_accel_less = 0x00000001, - ao_pyro_accel_greater = 0x00000002, - - ao_pyro_speed_less = 0x00000004, - ao_pyro_speed_greater = 0x00000008, - - ao_pyro_height_less = 0x00000010, - ao_pyro_height_greater = 0x00000020, - - ao_pyro_orient_less = 0x00000040, - ao_pyro_orient_greater = 0x00000080, - - ao_pyro_time_less = 0x00000100, - ao_pyro_time_greater = 0x00000200, - - ao_pyro_ascending = 0x00000400, - ao_pyro_descending = 0x00000800, - - ao_pyro_after_motor = 0x00001000, - - ao_pyro_delay = 0x00002000, - - ao_pyro_state_less = 0x00004000, - ao_pyro_state_greater_or_equal = 0x00008000, -}; - -struct ao_pyro { - enum ao_pyro_flag flags; - int16_t accel_less, accel_greater; - int16_t speed_less, speed_greater; - int16_t height_less, height_greater; - int16_t orient_less, orient_greater; - int16_t time_less, time_greater; - int16_t delay; - uint8_t state_less, state_greater_or_equal; - int16_t motor; - uint16_t delay_done; - uint8_t fired; -}; - -extern uint8_t ao_pyro_wakeup; - -extern uint16_t ao_pyro_fired; - -void -ao_pyro_set(void); - -void -ao_pyro_show(void); - -void -ao_pyro_init(void); - -void -ao_pyro_manual(uint8_t p); - -void -ao_pyro_print_status(void); - -#endif diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h deleted file mode 100644 index 044f1607..00000000 --- a/src/core/ao_quaternion.h +++ /dev/null @@ -1,249 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_QUATERNION_H_ -#define _AO_QUATERNION_H_ - -#include - -struct ao_quaternion { - float r; /* real bit */ - float x, y, z; /* imaginary bits */ -}; - -static inline void ao_quaternion_multiply(struct ao_quaternion *r, - const struct ao_quaternion *a, - const struct ao_quaternion *b) -{ - struct ao_quaternion t; -#define T(_a,_b) (((a)->_a) * ((b)->_b)) - -/* - * Quaternions - * - * ii = jj = kk = ijk = -1; - * - * kji = 1; - * - * ij = k; ji = -k; - * kj = -i; jk = i; - * ik = -j; ki = j; - * - * Multiplication p * q: - * - * (pr + ipx + jpy + kpz) (qr + iqx + jqy + kqz) = - * - * ( pr * qr + pr * iqx + pr * jqy + pr * kqz) + - * (ipx * qr + ipx * iqx + ipx * jqy + ipx * kqz) + - * (jpy * qr + jpy * iqx + jpy * jqy + jpy * kqz) + - * (kpz * qr + kpz * iqx + kpz * jqy + kpz * kqz) = - * - * - * (pr * qr) + i(pr * qx) + j(pr * qy) + k(pr * qz) + - * i(px * qr) - (px * qx) + k(px * qy) - j(px * qz) + - * j(py * qr) - k(py * qx) - (py * qy) + i(py * qz) + - * k(pz * qr) + j(pz * qx) - i(pz * qy) - (pz * qz) = - * - * 1 * ( (pr * qr) - (px * qx) - (py * qy) - (pz * qz) ) + - * i * ( (pr * qx) + (px * qr) + (py * qz) - (pz * qy) ) + - * j * ( (pr * qy) - (px * qz) + (py * qr) + (pz * qx) ) + - * k * ( (pr * qz) + (px * qy) - (py * qx) + (pz * qr); - */ - - t.r = T(r,r) - T(x,x) - T(y,y) - T(z,z); - t.x = T(r,x) + T(x,r) + T(y,z) - T(z,y); - t.y = T(r,y) - T(x,z) + T(y,r) + T(z,x); - t.z = T(r,z) + T(x,y) - T(y,x) + T(z,r); -#undef T - *r = t; -} - -static inline void ao_quaternion_conjugate(struct ao_quaternion *r, - const struct ao_quaternion *a) -{ - r->r = a->r; - r->x = -a->x; - r->y = -a->y; - r->z = -a->z; -} - -static inline float ao_quaternion_normal(const struct ao_quaternion *a) -{ -#define S(_a) (((a)->_a) * ((a)->_a)) - return S(r) + S(x) + S(y) + S(z); -#undef S -} - -static inline void ao_quaternion_scale(struct ao_quaternion *r, - const struct ao_quaternion *a, - float b) -{ - r->r = a->r * b; - r->x = a->x * b; - r->y = a->y * b; - r->z = a->z * b; -} - -static inline void ao_quaternion_normalize(struct ao_quaternion *r, - const struct ao_quaternion *a) -{ - float n = ao_quaternion_normal(a); - - if (n > 0) - ao_quaternion_scale(r, a, 1/sqrtf(n)); - else - *r = *a; -} - -static inline float ao_quaternion_dot(const struct ao_quaternion *a, - const struct ao_quaternion *b) -{ -#define T(_a) (((a)->_a) * ((b)->_a)) - return T(r) + T(x) + T(y) + T(z); -#undef T -} - - -static inline void ao_quaternion_rotate(struct ao_quaternion *r, - const struct ao_quaternion *a, - const struct ao_quaternion *b) -{ - struct ao_quaternion c; - struct ao_quaternion t; - - ao_quaternion_multiply(&t, b, a); - ao_quaternion_conjugate(&c, b); - ao_quaternion_multiply(r, &t, &c); -} - -/* - * Compute a rotation quaternion between two vectors - * - * cos(θ) + u * sin(θ) - * - * where θ is the angle between the two vectors and u - * is a unit vector axis of rotation - */ - -static inline void ao_quaternion_vectors_to_rotation(struct ao_quaternion *r, - const struct ao_quaternion *a, - const struct ao_quaternion *b) -{ - /* - * The cross product will point orthogonally to the two - * vectors, forming our rotation axis. The length will be - * sin(θ), so these values are already multiplied by that. - */ - - float x = a->y * b->z - a->z * b->y; - float y = a->z * b->x - a->x * b->z; - float z = a->x * b->y - a->y * b->x; - - float s_2 = x*x + y*y + z*z; - float s = sqrtf(s_2); - - /* cos(θ) = a · b / (|a| |b|). - * - * a and b are both unit vectors, so the divisor is one - */ - float c = a->x*b->x + a->y*b->y + a->z*b->z; - - float c_half = sqrtf ((1 + c) / 2); - float s_half = sqrtf ((1 - c) / 2); - - /* - * Divide out the sine factor from the - * cross product, then multiply in the - * half sine factor needed for the quaternion - */ - float s_scale = s_half / s; - - r->x = x * s_scale; - r->y = y * s_scale; - r->z = z * s_scale; - - r->r = c_half; - - ao_quaternion_normalize(r, r); -} - -static inline void ao_quaternion_init_vector(struct ao_quaternion *r, - float x, float y, float z) -{ - r->r = 0; - r->x = x; - r->y = y; - r->z = z; -} - -static inline void ao_quaternion_init_rotation(struct ao_quaternion *r, - float x, float y, float z, - float s, float c) -{ - r->r = c; - r->x = s * x; - r->y = s * y; - r->z = s * z; -} - -static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r) -{ - r->r = 1; - r->x = r->y = r->z = 0; -} - -/* - * The sincosf from newlib just calls sinf and cosf. This is a bit - * faster, if slightly less precise - */ - -static inline void -ao_sincosf(float a, float *s, float *c) { - float _s = sinf(a); - *s = _s; - *c = sqrtf(1 - _s*_s); -} - -/* - * Initialize a quaternion from 1/2 euler rotation angles (in radians). - * - * Yes, it would be nicer if there were a faster way, but because we - * sample the gyros at only 100Hz, we end up getting angles too large - * to take advantage of sin(x) ≃ x. - * - * We might be able to use just a couple of elements of the sin taylor - * series though, instead of the whole sin function? - */ - -static inline void ao_quaternion_init_half_euler(struct ao_quaternion *r, - float x, float y, float z) -{ - float s_x, c_x; - float s_y, c_y; - float s_z, c_z; - - ao_sincosf(x, &s_x, &c_x); - ao_sincosf(y, &s_y, &c_y); - ao_sincosf(z, &s_z, &c_z); - - r->r = c_x * c_y * c_z + s_x * s_y * s_z; - r->x = s_x * c_y * c_z - c_x * s_y * s_z; - r->y = c_x * s_y * c_z + s_x * c_y * s_z; - r->z = c_x * c_y * s_z - s_x * s_y * c_z; -} - -#endif /* _AO_QUATERNION_H_ */ diff --git a/src/core/ao_radio_cmac.c b/src/core/ao_radio_cmac.c deleted file mode 100644 index bff848f6..00000000 --- a/src/core/ao_radio_cmac.c +++ /dev/null @@ -1,164 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include - -static __xdata uint8_t ao_radio_cmac_mutex; -__pdata int8_t ao_radio_cmac_rssi; -static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN]; - -static uint8_t -round_len(uint8_t len) -{ - uint8_t rem; - - /* Make sure we transfer at least one packet, and - * then make sure every packet is full. Note that - * there is no length encoded, and that the receiver - * must deal with any extra bytes in the packet - */ - if (len < AO_CMAC_KEY_LEN) - len = AO_CMAC_KEY_LEN; - rem = len % AO_CMAC_KEY_LEN; - if (rem != 0) - len += (AO_CMAC_KEY_LEN - rem); - return len; -} - -/* - * Sign and deliver the data sitting in the cmac buffer - */ -static void -radio_cmac_send(uint8_t len) __reentrant -{ - uint8_t i; - - len = round_len(len); - /* Make sure the AES key is loaded */ - ao_config_get(); - -#if HAS_MONITOR - ao_monitor_set(0); -#endif - - ao_mutex_get(&ao_aes_mutex); - ao_aes_set_mode(ao_aes_mode_cbc_mac); - ao_aes_set_key(ao_config.aes_key); - ao_aes_zero_iv(); - for (i = 0; i < len; i += AO_CMAC_KEY_LEN) { - if (i + AO_CMAC_KEY_LEN < len) - ao_aes_run(&cmac_data[i], NULL); - else - ao_aes_run(&cmac_data[i], &cmac_data[len]); - } - ao_mutex_put(&ao_aes_mutex); - - ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN); -} - -/* - * Receive and validate an incoming packet - */ - -static int8_t -radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant -{ - uint8_t i; - - len = round_len(len); -#if HAS_MONITOR - ao_monitor_set(0); -#endif - i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2, timeout); - - if (!i) { - ao_radio_cmac_rssi = 0; - return AO_RADIO_CMAC_TIMEOUT; - } - - ao_radio_cmac_rssi = ao_radio_rssi; - if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & AO_RADIO_STATUS_CRC_OK)) - return AO_RADIO_CMAC_CRC_ERROR; - - ao_config_get(); - - /* Compute the packet signature - */ - ao_mutex_get(&ao_aes_mutex); - ao_aes_set_mode(ao_aes_mode_cbc_mac); - ao_aes_set_key(ao_config.aes_key); - ao_aes_zero_iv(); - for (i = 0; i < len; i += AO_CMAC_KEY_LEN) { - if (i + AO_CMAC_KEY_LEN < len) - ao_aes_run(&cmac_data[i], NULL); - else - ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]); - } - ao_mutex_put(&ao_aes_mutex); - - /* Check the packet signature against the signature provided - * over the link - */ - - if (memcmp(&cmac_data[len], - &cmac_data[len + AO_CMAC_KEY_LEN + 2], - AO_CMAC_KEY_LEN) != 0) { - return AO_RADIO_CMAC_MAC_ERROR; - } - - return AO_RADIO_CMAC_OK; -} - -int8_t -ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant -{ - if (len > AO_CMAC_MAX_LEN) - return AO_RADIO_CMAC_LEN_ERROR; - ao_mutex_get(&ao_radio_cmac_mutex); - ao_xmemcpy(cmac_data, packet, len); -#if AO_LED_TX - ao_led_on(AO_LED_TX); -#endif - radio_cmac_send(len); -#if AO_LED_TX - ao_led_off(AO_LED_TX); -#endif - ao_mutex_put(&ao_radio_cmac_mutex); - return AO_RADIO_CMAC_OK; -} - -int8_t -ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant -{ - int8_t i; - if (len > AO_CMAC_MAX_LEN) - return AO_RADIO_CMAC_LEN_ERROR; - ao_mutex_get(&ao_radio_cmac_mutex); -#if AO_LED_RX - ao_led_on(AO_LED_RX); -#endif - i = radio_cmac_recv(len, timeout); -#if AO_LED_RX - ao_led_off(AO_LED_RX); -#endif - if (i == AO_RADIO_CMAC_OK) - ao_xmemcpy(packet, cmac_data, len); - ao_mutex_put(&ao_radio_cmac_mutex); - return i; -} - diff --git a/src/core/ao_radio_cmac.h b/src/core/ao_radio_cmac.h deleted file mode 100644 index e86f31e9..00000000 --- a/src/core/ao_radio_cmac.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_RADIO_CMAC_H_ -#define _AO_RADIO_CMAC_H_ - -#include - -#define AO_CMAC_KEY_LEN AO_AES_LEN -#define AO_CMAC_MAX_LEN (128 - AO_CMAC_KEY_LEN) - -extern __pdata int8_t ao_radio_cmac_rssi; - -int8_t -ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant; - -#define AO_RADIO_CMAC_OK 0 -#define AO_RADIO_CMAC_LEN_ERROR -1 -#define AO_RADIO_CMAC_CRC_ERROR -2 -#define AO_RADIO_CMAC_MAC_ERROR -3 -#define AO_RADIO_CMAC_TIMEOUT -4 - -int8_t -ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant; - -void -ao_radio_cmac_init(void); - -#endif /* _AO_RADIO_CMAC_H_ */ diff --git a/src/core/ao_radio_cmac_cmd.c b/src/core/ao_radio_cmac_cmd.c deleted file mode 100644 index 64410921..00000000 --- a/src/core/ao_radio_cmac_cmd.c +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include -#include - -static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN]; - -static uint8_t -getnibble(void) -{ - int8_t b; - - b = ao_cmd_hexchar(getchar()); - if (b < 0) { - ao_cmd_status = ao_cmd_lex_error; - return 0; - } - return (uint8_t) b; -} - -static uint8_t -getbyte(void) -{ - uint8_t b; - b = getnibble() << 4; - b |= getnibble(); - return b; -} - -static void -radio_cmac_send_cmd(void) __reentrant -{ - uint8_t i; - uint8_t len; - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - len = ao_cmd_lex_i; - if (len > AO_CMAC_MAX_LEN) { - ao_cmd_status = ao_cmd_syntax_error; - return; - } - flush(); - len = ao_cmd_lex_i; - for (i = 0; i < len; i++) { - cmac_data[i] = getbyte(); - if (ao_cmd_status != ao_cmd_success) - return; - } - ao_radio_cmac_send(cmac_data, len); -} - -static void -radio_cmac_recv_cmd(void) __reentrant -{ - uint8_t len, i; - uint16_t timeout; - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - len = ao_cmd_lex_i; - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - timeout = AO_MS_TO_TICKS(ao_cmd_lex_i); - i = ao_radio_cmac_recv(cmac_data, len, timeout); - if (i == AO_RADIO_CMAC_OK) { - printf ("PACKET "); - for (i = 0; i < len; i++) - printf("%02x", cmac_data[i]); - printf (" %d\n", ao_radio_cmac_rssi); - } else - printf ("ERROR %d %d\n", i, ao_radio_cmac_rssi); -} - -static __code struct ao_cmds ao_radio_cmac_cmds[] = { - { radio_cmac_send_cmd, "s \0Send AES-CMAC packet. Bytes to send follow on next line" }, - { radio_cmac_recv_cmd, "S \0Receive AES-CMAC packet. Timeout in ms" }, - { 0, NULL }, -}; - -void -ao_radio_cmac_cmd_init(void) -{ - ao_cmd_register(&ao_radio_cmac_cmds[0]); -} diff --git a/src/core/ao_radio_cmac_cmd.h b/src/core/ao_radio_cmac_cmd.h deleted file mode 100644 index 6b8782de..00000000 --- a/src/core/ao_radio_cmac_cmd.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_RADIO_CMAC_CMD_H_ -#define _AO_RADIO_CMAC_CMD_H_ - -void -ao_radio_cmac_cmd_init(void); - -#endif /* _AO_RADIO_CMAC_CMD_H_ */ diff --git a/src/core/ao_report.c b/src/core/ao_report.c deleted file mode 100644 index 1104cd82..00000000 --- a/src/core/ao_report.c +++ /dev/null @@ -1,198 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include -#include - -#define BIT(i,x) ((x) ? (1 << (i)) : 0) -#define MORSE1(a) (1 | BIT(3,a)) -#define MORSE2(a,b) (2 | BIT(3,a) | BIT(4,b)) -#define MORSE3(a,b,c) (3 | BIT(3,a) | BIT(4,b) | BIT(5,c)) -#define MORSE4(a,b,c,d) (4 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d)) -#define MORSE5(a,b,c,d,e) (5 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d) | BIT(7,e)) - -static const uint8_t flight_reports[] = { - MORSE3(0,0,0), /* startup, 'S' */ - MORSE2(0,0), /* idle 'I' */ - MORSE4(0,1,1,0), /* pad 'P' */ - MORSE4(1,0,0,0), /* boost 'B' */ - MORSE4(0,0,1,0), /* fast 'F' */ - MORSE4(1,0,1,0), /* coast 'C' */ - MORSE3(1,0,0), /* drogue 'D' */ - MORSE2(1,1), /* main 'M' */ - MORSE4(0,1,0,0), /* landed 'L' */ - MORSE4(1,0,0,1), /* invalid 'X' */ -}; - -#if HAS_BEEP -#define low(time) ao_beep_for(AO_BEEP_LOW, time) -#define mid(time) ao_beep_for(AO_BEEP_MID, time) -#define high(time) ao_beep_for(AO_BEEP_HIGH, time) -#else -#define low(time) ao_led_for(AO_LED_GREEN, time) -#define mid(time) ao_led_for(AO_LED_RED, time) -#define high(time) ao_led_for(AO_LED_GREEN|AO_LED_RED, time) -#endif -#define pause(time) ao_delay(time) - -static __pdata enum ao_flight_state ao_report_state; - -static void -ao_report_beep(void) __reentrant -{ - uint8_t r = flight_reports[ao_flight_state]; - uint8_t l = r & 7; - - if (!r) - return; - while (l--) { - if (r & 8) - mid(AO_MS_TO_TICKS(600)); - else - mid(AO_MS_TO_TICKS(200)); - pause(AO_MS_TO_TICKS(200)); - r >>= 1; - } - pause(AO_MS_TO_TICKS(400)); -} - -static void -ao_report_digit(uint8_t digit) __reentrant -{ - if (!digit) { - mid(AO_MS_TO_TICKS(500)); - pause(AO_MS_TO_TICKS(200)); - } else { - while (digit--) { - mid(AO_MS_TO_TICKS(200)); - pause(AO_MS_TO_TICKS(200)); - } - } - pause(AO_MS_TO_TICKS(300)); -} - -static void -ao_report_altitude(void) -{ - __pdata int16_t agl = ao_max_height; - __xdata uint8_t digits[10]; - __pdata uint8_t ndigits, i; - - if (agl < 0) - agl = 0; - ndigits = 0; - do { - digits[ndigits++] = agl % 10; - agl /= 10; - } while (agl); - - i = ndigits; - do - ao_report_digit(digits[--i]); - while (i != 0); -} - -#if HAS_IGNITE_REPORT -static uint8_t -ao_report_igniter_ready(enum ao_igniter igniter) -{ - return ao_igniter_status(igniter) == ao_igniter_ready ? 1 : 0; -} - -uint8_t -ao_report_igniter(void) -{ - return (ao_report_igniter_ready(ao_igniter_drogue) | - (ao_report_igniter_ready(ao_igniter_main) << 1)); -} - -static void -ao_report_continuity(void) __reentrant -{ - uint8_t c; - -#if !HAS_IGNITE - if (!ao_igniter_present) - return; -#endif - c = ao_report_igniter(); - if (c) { - while (c--) { - high(AO_MS_TO_TICKS(25)); - pause(AO_MS_TO_TICKS(100)); - } - } else { - c = 10; - while (c--) { - high(AO_MS_TO_TICKS(20)); - low(AO_MS_TO_TICKS(20)); - } - } -#if HAS_LOG - if (ao_log_full()) { - pause(AO_MS_TO_TICKS(100)); - c = 2; - while (c--) { - low(AO_MS_TO_TICKS(100)); - mid(AO_MS_TO_TICKS(100)); - high(AO_MS_TO_TICKS(100)); - mid(AO_MS_TO_TICKS(100)); - } - } -#endif -} -#endif - -void -ao_report(void) -{ - ao_report_state = ao_flight_state; - for(;;) { - ao_report_beep(); - if (ao_flight_state == ao_flight_landed) { - ao_report_altitude(); -#if HAS_FLIGHT - ao_delay(AO_SEC_TO_TICKS(5)); - continue; -#endif - } -#if HAS_IGNITE_REPORT - if (ao_flight_state == ao_flight_idle) - ao_report_continuity(); - while (ao_flight_state == ao_flight_pad) { - uint8_t c; - ao_report_continuity(); - c = 50; - while (c-- && ao_flight_state == ao_flight_pad) - pause(AO_MS_TO_TICKS(100)); - } -#endif - - while (ao_report_state == ao_flight_state) - ao_sleep(DATA_TO_XDATA(&ao_flight_state)); - ao_report_state = ao_flight_state; - } -} - -static __xdata struct ao_task ao_report_task; - -void -ao_report_init(void) -{ - ao_add_task(&ao_report_task, ao_report, "report"); -} diff --git a/src/core/ao_report_micro.c b/src/core/ao_report_micro.c deleted file mode 100644 index 0e8e287f..00000000 --- a/src/core/ao_report_micro.c +++ /dev/null @@ -1,57 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include - -#define mid(time) ao_led_for(AO_LED_REPORT, time) -#define pause(time) ao_delay(time) - -static void -ao_report_digit(uint8_t digit) __reentrant -{ - if (!digit) { - mid(AO_MS_TO_TICKS(1000)); - pause(AO_MS_TO_TICKS(300)); - } else { - while (digit--) { - mid(AO_MS_TO_TICKS(300)); - pause(AO_MS_TO_TICKS(300)); - } - } - pause(AO_MS_TO_TICKS(1000)); -} - -void -ao_report_altitude(void) -{ - __pdata alt_t agl = ao_max_height; - static __xdata uint8_t digits[11]; - __pdata uint8_t ndigits, i; - - if (agl < 0) - agl = 0; - ndigits = 0; - do { - digits[ndigits++] = agl % 10; - agl /= 10; - } while (agl); - - i = ndigits; - do - ao_report_digit(digits[--i]); - while (i != 0); -} diff --git a/src/core/ao_rssi.c b/src/core/ao_rssi.c deleted file mode 100644 index 244a84fe..00000000 --- a/src/core/ao_rssi.c +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -static __xdata uint16_t ao_rssi_time; -static __pdata uint16_t ao_rssi_delay; -static __pdata uint8_t ao_rssi_led; - -void -ao_rssi(void) -{ - for (;;) { - while ((int16_t) (ao_time() - ao_rssi_time) > AO_SEC_TO_TICKS(3)) - ao_sleep(&ao_rssi_time); - ao_led_for(ao_rssi_led, AO_MS_TO_TICKS(100)); - ao_delay(ao_rssi_delay); - } -} - -void -ao_rssi_set(int rssi_value) -{ - if (rssi_value > 0) - rssi_value = 0; - ao_rssi_delay = AO_MS_TO_TICKS((-rssi_value) * 5); - ao_rssi_time = ao_time(); - ao_wakeup(&ao_rssi_time); -} - -__xdata struct ao_task ao_rssi_task; - -void -ao_rssi_init(uint8_t rssi_led) -{ - ao_rssi_led = rssi_led; - ao_rssi_delay = 0; - ao_add_task(&ao_rssi_task, ao_rssi, "rssi"); -} diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c deleted file mode 100644 index 34658951..00000000 --- a/src/core/ao_sample.c +++ /dev/null @@ -1,372 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_FLIGHT_TEST -#include "ao.h" -#include -#endif - -#if HAS_GYRO -#include -#endif - -/* - * Current sensor values - */ - -#ifndef PRES_TYPE -#define PRES_TYPE int32_t -#define ALT_TYPE int32_t -#define ACCEL_TYPE int16_t -#endif - -__pdata uint16_t ao_sample_tick; /* time of last data */ -__pdata pres_t ao_sample_pres; -__pdata alt_t ao_sample_alt; -__pdata alt_t ao_sample_height; -#if HAS_ACCEL -__pdata accel_t ao_sample_accel; -#endif -#if HAS_GYRO -__pdata accel_t ao_sample_accel_along; -__pdata accel_t ao_sample_accel_across; -__pdata accel_t ao_sample_accel_through; -__pdata gyro_t ao_sample_roll; -__pdata gyro_t ao_sample_pitch; -__pdata gyro_t ao_sample_yaw; -__pdata angle_t ao_sample_orient; -#endif - -__data uint8_t ao_sample_data; - -/* - * Sensor calibration values - */ - -__pdata pres_t ao_ground_pres; /* startup pressure */ -__pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ - -#if HAS_ACCEL -__pdata accel_t ao_ground_accel; /* startup acceleration */ -__pdata accel_t ao_accel_2g; /* factory accel calibration */ -__pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ -#endif - -#if HAS_GYRO -__pdata accel_t ao_ground_accel_along; -__pdata accel_t ao_ground_accel_across; -__pdata accel_t ao_ground_accel_through; -__pdata int32_t ao_ground_pitch; -__pdata int32_t ao_ground_yaw; -__pdata int32_t ao_ground_roll; -#endif - -static __pdata uint8_t ao_preflight; /* in preflight mode */ - -static __pdata uint16_t nsamples; -__pdata int32_t ao_sample_pres_sum; -#if HAS_ACCEL -__pdata int32_t ao_sample_accel_sum; -#endif -#if HAS_GYRO -__pdata int32_t ao_sample_accel_along_sum; -__pdata int32_t ao_sample_accel_across_sum; -__pdata int32_t ao_sample_accel_through_sum; -__pdata int32_t ao_sample_pitch_sum; -__pdata int32_t ao_sample_yaw_sum; -__pdata int32_t ao_sample_roll_sum; -static struct ao_quaternion ao_rotation; -#endif - -#if HAS_FLIGHT_DEBUG -extern uint8_t ao_orient_test; -#endif - -static void -ao_sample_preflight_add(void) -{ -#if HAS_ACCEL - ao_sample_accel_sum += ao_sample_accel; -#endif - ao_sample_pres_sum += ao_sample_pres; -#if HAS_GYRO - ao_sample_accel_along_sum += ao_sample_accel_along; - ao_sample_accel_across_sum += ao_sample_accel_across; - ao_sample_accel_through_sum += ao_sample_accel_through; - ao_sample_pitch_sum += ao_sample_pitch; - ao_sample_yaw_sum += ao_sample_yaw; - ao_sample_roll_sum += ao_sample_roll; -#endif - ++nsamples; -} - -static void -ao_sample_preflight_set(void) -{ -#if HAS_ACCEL - ao_ground_accel = ao_sample_accel_sum >> 9; - ao_sample_accel_sum = 0; -#endif - ao_ground_pres = ao_sample_pres_sum >> 9; - ao_ground_height = pres_to_altitude(ao_ground_pres); - ao_sample_pres_sum = 0; -#if HAS_GYRO - ao_ground_accel_along = ao_sample_accel_along_sum >> 9; - ao_ground_accel_across = ao_sample_accel_across_sum >> 9; - ao_ground_accel_through = ao_sample_accel_through_sum >> 9; - ao_ground_pitch = ao_sample_pitch_sum; - ao_ground_yaw = ao_sample_yaw_sum; - ao_ground_roll = ao_sample_roll_sum; - ao_sample_accel_along_sum = 0; - ao_sample_accel_across_sum = 0; - ao_sample_accel_through_sum = 0; - ao_sample_pitch_sum = 0; - ao_sample_yaw_sum = 0; - ao_sample_roll_sum = 0; - ao_sample_orient = 0; - - struct ao_quaternion orient; - - /* Take the pad IMU acceleration values and compute our current direction - */ - - ao_quaternion_init_vector(&orient, - (ao_ground_accel_across - ao_config.accel_zero_across), - (ao_ground_accel_through - ao_config.accel_zero_through), - (ao_ground_accel_along - ao_config.accel_zero_along)); - - ao_quaternion_normalize(&orient, - &orient); - - /* Here's up */ - - struct ao_quaternion up = { .r = 0, .x = 0, .y = 0, .z = 1 }; - - if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) - up.z = -1; - - /* Compute rotation to get from up to our current orientation, set - * that as the current rotation vector - */ - ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient); -#if HAS_FLIGHT_DEBUG - if (ao_orient_test) - printf("\n\treset\n"); -#endif -#endif - nsamples = 0; -} - -#if HAS_GYRO - -#define TIME_DIV 200.0f - -static void -ao_sample_rotate(void) -{ -#ifdef AO_FLIGHT_TEST - float dt = (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; -#else - static const float dt = 1/TIME_DIV; -#endif - float x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt; - float y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt; - float z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt; - struct ao_quaternion rot; - - ao_quaternion_init_half_euler(&rot, x, y, z); - ao_quaternion_multiply(&ao_rotation, &rot, &ao_rotation); - - /* And normalize to make sure it remains a unit vector */ - ao_quaternion_normalize(&ao_rotation, &ao_rotation); - - /* Compute pitch angle from vertical by taking the pad - * orientation vector and rotating it by the current total - * rotation value. That will be a unit vector pointing along - * the airframe axis. The Z value will be the cosine of the - * change in the angle from vertical since boost. - * - * rot = ao_rotation * vertical * ao_rotation° - * rot = ao_rotation * (0,0,0,1) * ao_rotation° - * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z - * - * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) - * = a.z² - a.y² - a.x² + a.r² - * - * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° - * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z - * - * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) - * = -a.z² + a.y² + a.x² - a.r² - */ - - float rotz; - rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r; - - ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI); - -#if HAS_FLIGHT_DEBUG - if (ao_orient_test) { - printf ("rot %d %d %d orient %d \r", - (int) (x * 1000), - (int) (y * 1000), - (int) (z * 1000), - ao_sample_orient); - } -#endif - -} -#endif - -static void -ao_sample_preflight(void) -{ - /* startup state: - * - * Collect 512 samples of acceleration and pressure - * data and average them to find the resting values - */ - if (nsamples < 512) { - ao_sample_preflight_add(); - } else { -#if HAS_ACCEL - ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; - ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g; -#endif - ao_sample_preflight_set(); - ao_preflight = FALSE; - } -} - -/* - * While in pad mode, constantly update the ground state by - * re-averaging the data. This tracks changes in orientation, which - * might be caused by adjustments to the rocket on the pad and - * pressure, which might be caused by changes in the weather. - */ - -static void -ao_sample_preflight_update(void) -{ - if (nsamples < 512) - ao_sample_preflight_add(); - else if (nsamples < 1024) - ++nsamples; - else - ao_sample_preflight_set(); -} - -#if 0 -#if HAS_GYRO -static int32_t p_filt; -static int32_t y_filt; - -static gyro_t inline ao_gyro(void) { - gyro_t p = ao_sample_pitch - ao_ground_pitch; - gyro_t y = ao_sample_yaw - ao_ground_yaw; - - p_filt = p_filt - (p_filt >> 6) + p; - y_filt = y_filt - (y_filt >> 6) + y; - - p = p_filt >> 6; - y = y_filt >> 6; - return ao_sqrt(p*p + y*y); -} -#endif -#endif - -uint8_t -ao_sample(void) -{ - ao_wakeup(DATA_TO_XDATA(&ao_sample_data)); - ao_sleep((void *) DATA_TO_XDATA(&ao_data_head)); - while (ao_sample_data != ao_data_head) { - __xdata struct ao_data *ao_data; - - /* Capture a sample */ - ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data]; - ao_sample_tick = ao_data->tick; - -#if HAS_BARO - ao_data_pres_cook(ao_data); - ao_sample_pres = ao_data_pres(ao_data); - ao_sample_alt = pres_to_altitude(ao_sample_pres); - ao_sample_height = ao_sample_alt - ao_ground_height; -#endif - -#if HAS_ACCEL - ao_sample_accel = ao_data_accel_cook(ao_data); - if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) - ao_sample_accel = ao_data_accel_invert(ao_sample_accel); - ao_data_set_accel(ao_data, ao_sample_accel); -#endif -#if HAS_GYRO - ao_sample_accel_along = ao_data_along(ao_data); - ao_sample_accel_across = ao_data_across(ao_data); - ao_sample_accel_through = ao_data_through(ao_data); - ao_sample_pitch = ao_data_pitch(ao_data); - ao_sample_yaw = ao_data_yaw(ao_data); - ao_sample_roll = ao_data_roll(ao_data); -#endif - - if (ao_preflight) - ao_sample_preflight(); - else { - if (ao_flight_state < ao_flight_boost) - ao_sample_preflight_update(); - ao_kalman(); -#if HAS_GYRO - ao_sample_rotate(); -#endif - } -#ifdef AO_FLIGHT_TEST - ao_sample_prev_tick = ao_sample_tick; -#endif - ao_sample_data = ao_data_ring_next(ao_sample_data); - } - return !ao_preflight; -} - -void -ao_sample_init(void) -{ - ao_config_get(); - nsamples = 0; - ao_sample_pres_sum = 0; - ao_sample_pres = 0; -#if HAS_ACCEL - ao_sample_accel_sum = 0; - ao_sample_accel = 0; -#endif -#if HAS_GYRO - ao_sample_accel_along_sum = 0; - ao_sample_accel_across_sum = 0; - ao_sample_accel_through_sum = 0; - ao_sample_accel_along = 0; - ao_sample_accel_across = 0; - ao_sample_accel_through = 0; - ao_sample_pitch_sum = 0; - ao_sample_yaw_sum = 0; - ao_sample_roll_sum = 0; - ao_sample_pitch = 0; - ao_sample_yaw = 0; - ao_sample_roll = 0; - ao_sample_orient = 0; -#endif - ao_sample_data = ao_data_head; - ao_preflight = TRUE; -} diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h deleted file mode 100644 index 16d4c507..00000000 --- a/src/core/ao_sample.h +++ /dev/null @@ -1,156 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_SAMPLE_H_ -#define _AO_SAMPLE_H_ - -#include - -/* - * ao_sample.c - */ - -/* - * Barometer calibration - * - * We directly sample the barometer. The specs say: - * - * Pressure range: 15-115 kPa - * Voltage at 115kPa: 2.82 - * Output scale: 27mV/kPa - * - * If we want to detect launch with the barometer, we need - * a large enough bump to not be fooled by noise. At typical - * launch elevations (0-2000m), a 200Pa pressure change cooresponds - * to about a 20m elevation change. This is 5.4mV, or about 3LSB. - * As all of our calculations are done in 16 bits, we'll actually see a change - * of 16 times this though - * - * 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa - */ - -/* Accelerometer calibration - * - * We're sampling the accelerometer through a resistor divider which - * consists of 5k and 10k resistors. This multiplies the values by 2/3. - * That goes into the cc1111 A/D converter, which is running at 11 bits - * of precision with the bits in the MSB of the 16 bit value. Only positive - * values are used, so values should range from 0-32752 for 0-3.3V. The - * specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what - * the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV, - * for a final computation of: - * - * 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g - * - * Zero g was measured at 16000 (we would expect 16384). - * Note that this value is only require to tell if the - * rocket is standing upright. Once that is determined, - * the value of the accelerometer is averaged for 100 samples - * to find the resting accelerometer value, which is used - * for all further flight computations - */ - -/* - * Above this height, the baro sensor doesn't work - */ -#if HAS_MS5607 -#define AO_MAX_BARO_HEIGHT 30000 -#else -#define AO_MAX_BARO_HEIGHT 12000 -#endif - -/* - * Above this speed, baro measurements are unreliable - */ -#define AO_MAX_BARO_SPEED 200 - -#define ACCEL_NOSE_UP (ao_accel_2g >> 2) - -/* - * Speed and acceleration are scaled by 16 to provide a bit more - * resolution while still having reasonable range. Note that this - * limits speed to 2047m/s (around mach 6) and acceleration to - * 2047m/s² (over 200g) - */ - -#define AO_M_TO_HEIGHT(m) ((int16_t) (m)) -#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) -#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) - -extern __pdata uint16_t ao_sample_tick; /* time of last data */ -extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */ -extern __data uint8_t ao_sample_data; /* Ring position of last processed sample */ - -#if HAS_BARO -extern __pdata pres_t ao_sample_pres; /* most recent pressure sensor reading */ -extern __pdata alt_t ao_sample_alt; /* MSL of ao_sample_pres */ -extern __pdata alt_t ao_sample_height; /* AGL of ao_sample_pres */ -extern __pdata pres_t ao_ground_pres; /* startup pressure */ -extern __pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ -#endif - -#if HAS_ACCEL -extern __pdata accel_t ao_sample_accel; /* most recent accel sensor reading */ -extern __pdata accel_t ao_ground_accel; /* startup acceleration */ -extern __pdata accel_t ao_accel_2g; /* factory accel calibration */ -extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ -#endif -#if HAS_GYRO -extern __pdata accel_t ao_ground_accel_along; -extern __pdata accel_t ao_ground_accel_across; -extern __pdata accel_t ao_ground_accel_through; -extern __pdata int32_t ao_ground_pitch; /* * 512 */ -extern __pdata int32_t ao_ground_yaw; /* * 512 */ -extern __pdata int32_t ao_ground_roll; /* * 512 */ -extern __pdata accel_t ao_sample_accel_along; -extern __pdata accel_t ao_sample_accel_across; -extern __pdata accel_t ao_sample_accel_through; -extern __pdata gyro_t ao_sample_roll; -extern __pdata gyro_t ao_sample_pitch; -extern __pdata gyro_t ao_sample_yaw; -extern __pdata angle_t ao_sample_orient; -#endif - -void ao_sample_init(void); - -/* returns FALSE in preflight mode, TRUE in flight mode */ -uint8_t ao_sample(void); - -/* - * ao_kalman.c - */ - -#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) -#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) -#define from_fix(x) ((x) >> 16) - -extern __pdata int16_t ao_height; /* meters */ -extern __pdata int16_t ao_speed; /* m/s * 16 */ -extern __pdata int16_t ao_accel; /* m/s² * 16 */ -extern __xdata int16_t ao_max_height; /* max of ao_height */ -extern __xdata int16_t ao_avg_height; /* running average of height */ - -extern __pdata int16_t ao_error_h; -extern __pdata int16_t ao_error_h_sq_avg; - -#if HAS_ACCEL -extern __pdata int16_t ao_error_a; -#endif - -void ao_kalman(void); - -#endif /* _AO_SAMPLE_H_ */ diff --git a/src/core/ao_sample_profile.c b/src/core/ao_sample_profile.c deleted file mode 100644 index d3743d12..00000000 --- a/src/core/ao_sample_profile.c +++ /dev/null @@ -1,173 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include -#include - -#ifndef AO_SAMPLE_PROFILE_LOW_PC -#define AO_SAMPLE_PROFILE_LOW_PC 0x08002000 -#endif - -#ifndef AO_SAMPLE_PROFILE_HIGH_PC -#define AO_SAMPLE_PROFILE_HIGH_PC 0x0800f000 -#endif - -#ifndef AO_SAMPLE_PROFILE_SHIFT -#define AO_SAMPLE_PROFILE_SHIFT 6 -#endif - -#define AO_SAMPLE_PROFILE_RANGE (AO_SAMPLE_PROFILE_HIGH_PC - AO_SAMPLE_PROFILE_LOW_PC) -#define AO_SAMPLE_PROFILE_NUM (AO_SAMPLE_PROFILE_RANGE >> AO_SAMPLE_PROFILE_SHIFT) - -static uint16_t prev_tick; -static uint16_t samples[AO_SAMPLE_PROFILE_NUM]; -static uint8_t missed[AO_SAMPLE_PROFILE_NUM/8]; -static uint16_t max_miss; -static uint32_t task, isr, os, idle; - -extern uint8_t ao_idle_loc; - -void -ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr) -{ - uint16_t delta = tick - prev_tick; - - if (pc < AO_SAMPLE_PROFILE_LOW_PC) - return; - if (pc >= AO_SAMPLE_PROFILE_HIGH_PC) - return; - if (ao_cur_task) { - uint8_t *sp; - int32_t sp_delta; - - asm("mov %0,sp" : "=&r" (sp)); - sp_delta = sp - (uint8_t *) ao_cur_task->stack; - if (-96 < sp_delta && sp_delta < 16) - ao_panic(AO_PANIC_STACK); - } - - if (in_isr) - isr += delta; - else if (ao_cur_task) { - ao_cur_task->ticks += delta; - task += delta; - } else if (pc == (uint32_t) &ao_idle_loc) - idle += delta; - else - os += delta; - - pc -= AO_SAMPLE_PROFILE_LOW_PC; - pc >>= AO_SAMPLE_PROFILE_SHIFT; - samples[pc] += delta; - - if (delta > 1) - missed[pc >> 3] |= (1 << (pc & 7)); - if (delta > max_miss) - max_miss = delta; - prev_tick = tick; -} - -static void -ao_sample_profile_start(void) -{ - prev_tick = ao_sample_profile_timer_start(); -} - -static void -ao_sample_profile_stop(void) -{ - ao_sample_profile_timer_stop(); -} - -static void -ao_sample_profile_dump(void) -{ - uint16_t a; - uint8_t t; - - printf ("task %6d\n", task); - printf ("isr %6d\n", isr); - printf ("os %6d\n", os); - printf ("idle %6d\n", idle); - printf ("irq blocked %d\n", max_miss); - for (t = 0; t < ao_num_tasks; t++) - printf ("task %6d %6d %6d %s\n", - ao_tasks[t]->ticks, - ao_tasks[t]->yields, - ao_tasks[t]->max_run, - ao_tasks[t]->name); - for (a = 0; a < AO_SAMPLE_PROFILE_NUM; a++) { - if (samples[a]) - printf ("%04x %c %u\n", - (a << AO_SAMPLE_PROFILE_SHIFT) + AO_SAMPLE_PROFILE_LOW_PC, - missed[a >> 3] & (1 << (a & 7)) ? '*' : ' ', - samples[a]); - } -} - -static void -ao_sample_profile_clear(void) -{ - int t; - - task = isr = os = idle = 0; - max_miss = 0; - memset(samples, '\0', sizeof (samples)); - memset(missed, '\0', sizeof (missed)); - for (t = 0; t < ao_num_tasks; t++) { - ao_tasks[t]->ticks = 0; - ao_tasks[t]->yields = 0; - ao_tasks[t]->max_run = 0; - } -} - -static void -ao_sample_profile_cmd(void) -{ - ao_cmd_white(); - switch (ao_cmd_lex_c) { - case '1': - ao_sample_profile_start(); - break; - case '0': - ao_sample_profile_stop(); - break; - case 'd': - ao_sample_profile_dump(); - break; - case 'c': - ao_sample_profile_clear(); - break; - default: - ao_cmd_status = ao_cmd_syntax_error; - break; - } -} - -static __code struct ao_cmds ao_sample_profile_cmds[] = { - { ao_sample_profile_cmd, "S <1 start,0 stop, d dump,c clear>\0Sample profile" }, - { 0, NULL } -}; - -void -ao_sample_profile_init(void) -{ - ao_sample_profile_timer_init(); - ao_cmd_register(&ao_sample_profile_cmds[0]); - ao_sample_profile_clear(); -} diff --git a/src/core/ao_sample_profile.h b/src/core/ao_sample_profile.h deleted file mode 100644 index dbc29d3d..00000000 --- a/src/core/ao_sample_profile.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_SAMPLE_PROFILE_H_ -#define _AO_SAMPLE_PROFILE_H_ - -#include - -void -ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr); - -void -ao_sample_profile_init(void); - -#endif /* _AO_SAMPLE_PROFILE_H_ */ diff --git a/src/core/ao_send_packet.c b/src/core/ao_send_packet.c deleted file mode 100644 index 66315d22..00000000 --- a/src/core/ao_send_packet.c +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -#define AO_MAX_SEND 128 - -static __xdata uint8_t ao_send[AO_MAX_SEND]; - -static void -ao_send_packet(void) -{ - __pdata uint16_t count; - uint8_t b; - __pdata uint8_t i; - - ao_cmd_hex(); - count = ao_cmd_lex_i; - if (ao_cmd_status != ao_cmd_success) - return; - if (count > AO_MAX_SEND - 2) { - ao_cmd_status = ao_cmd_syntax_error; - return; - } - for (i = 0; i < count; i++) { - b = ao_getnibble() << 4; - b |= ao_getnibble(); - if (ao_cmd_status != ao_cmd_success) - return; - ao_send[i] = b; - } - ao_radio_send(ao_send, count); -} - -static __code struct ao_cmds ao_send_packet_cmds[] = { - { ao_send_packet, "S \0Send packet. Data on next line" }, - { 0, NULL } -}; - -void -ao_send_packet_init(void) -{ - ao_cmd_register(&ao_send_packet_cmds[0]); -} diff --git a/src/core/ao_send_packet.h b/src/core/ao_send_packet.h deleted file mode 100644 index 526f7b55..00000000 --- a/src/core/ao_send_packet.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_SEND_PACKET_H_ -#define _AO_SEND_PACKET_H_ - -void -ao_send_packet_init(void); - -#endif /* _AO_SEND_PACKET_H_ */ diff --git a/src/core/ao_serial.h b/src/core/ao_serial.h deleted file mode 100644 index baf213c0..00000000 --- a/src/core/ao_serial.h +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_SERIAL_H_ -#define _AO_SERIAL_H_ - -#define AO_SERIAL_SPEED_4800 0 -#define AO_SERIAL_SPEED_9600 1 -#define AO_SERIAL_SPEED_19200 2 -#define AO_SERIAL_SPEED_57600 3 -#define AO_SERIAL_SPEED_115200 4 - -#if HAS_SERIAL_0 -extern volatile __xdata struct ao_fifo ao_serial0_rx_fifo; -extern volatile __xdata struct ao_fifo ao_serial0_tx_fifo; - -char -ao_serial0_getchar(void); - -int -_ao_serial0_pollchar(void); - -void -ao_serial0_putchar(char c); - -void -ao_serial0_drain(void); - -void -ao_serial0_set_speed(uint8_t speed); -#endif - -#if HAS_SERIAL_1 -extern volatile __xdata struct ao_fifo ao_serial1_rx_fifo; -extern volatile __xdata struct ao_fifo ao_serial1_tx_fifo; - -char -ao_serial1_getchar(void); - -int -_ao_serial1_pollchar(void); - -void -ao_serial1_putchar(char c); - -void -ao_serial1_drain(void); - -void -ao_serial1_set_speed(uint8_t speed); -#endif - -#if HAS_SERIAL_2 -extern volatile __xdata struct ao_fifo ao_serial2_rx_fifo; -extern volatile __xdata struct ao_fifo ao_serial2_tx_fifo; - -char -ao_serial2_getchar(void); - -int -_ao_serial2_pollchar(void); - -void -ao_serial2_putchar(char c); - -void -ao_serial2_drain(void); - -void -ao_serial2_set_speed(uint8_t speed); -#endif - -#if HAS_SERIAL_3 -extern volatile __xdata struct ao_fifo ao_serial3_rx_fifo; -extern volatile __xdata struct ao_fifo ao_serial3_tx_fifo; - -char -ao_serial3_getchar(void); - -int -_ao_serial3_pollchar(void); - -void -ao_serial3_putchar(char c); - -void -ao_serial3_drain(void); - -void -ao_serial3_set_speed(uint8_t speed); -#endif - -void -ao_serial_init(void); - -#endif /* _AO_SERIAL_H_ */ diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c deleted file mode 100644 index 3a550eaa..00000000 --- a/src/core/ao_sqrt.c +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef AO_FLIGHT_TEST -#include "ao.h" -#endif - -/* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */ -/** - * int_sqrt - rough approximation to sqrt - * @x: integer of which to calculate the sqrt - * - * A very rough approximation to the sqrt() function. - */ - -uint32_t -ao_sqrt(uint32_t op) -{ - uint32_t res = 0; - uint32_t one = 1UL << (sizeof (one) * 8 - 2); - - while (one > op) - one >>= 2; - - while (one != 0) { - if (op >= res + one) { - op = op - (res + one); - res = res + 2 * one; - } - res /= 2; - one /= 4; - } - return res; -} diff --git a/src/core/ao_state.c b/src/core/ao_state.c deleted file mode 100644 index ed197aa5..00000000 --- a/src/core/ao_state.c +++ /dev/null @@ -1,23 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -const char const * const ao_state_names[] = { - "startup", "idle", "pad", "boost", "fast", - "coast", "drogue", "main", "landed", "invalid" -}; diff --git a/src/core/ao_stdio.c b/src/core/ao_stdio.c deleted file mode 100644 index 99118137..00000000 --- a/src/core/ao_stdio.c +++ /dev/null @@ -1,158 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" - -/* - * Basic I/O functions to support SDCC stdio package - */ - -#ifndef USE_SERIAL_0_STDIN -#define USE_SERIAL_0_STDIN 0 -#endif -#ifndef USE_SERIAL_1_STDIN -#define USE_SERIAL_1_STDIN 0 -#endif -#ifndef USE_SERIAL_2_STDIN -#define USE_SERIAL_2_STDIN 0 -#endif -#ifndef USE_SERIAL_3_STDIN -#define USE_SERIAL_3_STDIN 0 -#endif -#ifndef USE_SERIAL_4_STDIN -#define USE_SERIAL_4_STDIN 0 -#endif -#ifndef USE_SERIAL_5_STDIN -#define USE_SERIAL_5_STDIN 0 -#endif -#ifndef USE_SERIAL_6_STDIN -#define USE_SERIAL_6_STDIN 0 -#endif -#ifndef USE_SERIAL_7_STDIN -#define USE_SERIAL_7_STDIN 0 -#endif -#ifndef USE_SERIAL_8_STDIN -#define USE_SERIAL_8_STDIN 0 -#endif -#ifndef USE_SERIAL_9_STDIN -#define USE_SERIAL_9_STDIN 0 -#endif -#ifndef PACKET_HAS_SLAVE -#define PACKET_HAS_SLAVE 0 -#endif - -#define USE_SERIAL_STDIN (USE_SERIAL_0_STDIN + \ - USE_SERIAL_1_STDIN + \ - USE_SERIAL_2_STDIN + \ - USE_SERIAL_3_STDIN + \ - USE_SERIAL_4_STDIN + \ - USE_SERIAL_5_STDIN + \ - USE_SERIAL_6_STDIN + \ - USE_SERIAL_7_STDIN + \ - USE_SERIAL_8_STDIN + \ - USE_SERIAL_9_STDIN) - -#define AO_NUM_STDIOS (HAS_USB + PACKET_HAS_SLAVE + USE_SERIAL_STDIN) - -__xdata struct ao_stdio ao_stdios[AO_NUM_STDIOS]; - -#if AO_NUM_STDIOS > 1 -__pdata int8_t ao_cur_stdio; -__pdata int8_t ao_num_stdios; -#else -__pdata int8_t ao_cur_stdio; -#define ao_cur_stdio 0 -#define ao_num_stdios 0 -#endif - -void -putchar(char c) -{ -#if LOW_LEVEL_DEBUG - if (!ao_cur_task) { - extern void ao_debug_out(char c); - if (c == '\n') - ao_debug_out('\r'); - ao_debug_out(c); - return; - } -#endif - if (c == '\n') - (*ao_stdios[ao_cur_stdio].putchar)('\r'); - (*ao_stdios[ao_cur_stdio].putchar)(c); -} - -void -flush(void) -{ - if (ao_stdios[ao_cur_stdio].flush) - ao_stdios[ao_cur_stdio].flush(); -} - -__xdata uint8_t ao_stdin_ready; - -char -getchar(void) __reentrant -{ - int c; - int8_t stdio; - - ao_arch_block_interrupts(); - stdio = ao_cur_stdio; - for (;;) { - c = ao_stdios[stdio]._pollchar(); - if (c != AO_READ_AGAIN) - break; -#if AO_NUM_STDIOS > 1 - if (++stdio == ao_num_stdios) - stdio = 0; - if (stdio == ao_cur_stdio) -#endif - ao_sleep(&ao_stdin_ready); - } -#if AO_NUM_STDIOS > 1 - ao_cur_stdio = stdio; -#endif - ao_arch_release_interrupts(); - return c; -} - -uint8_t -ao_echo(void) -{ - return ao_stdios[ao_cur_stdio].echo; -} - -int8_t -ao_add_stdio(int (*_pollchar)(void), - void (*putchar)(char), - void (*flush)(void)) __reentrant -{ -#if AO_NUM_STDIOS > 1 - if (ao_num_stdios == AO_NUM_STDIOS) - ao_panic(AO_PANIC_STDIO); -#endif - ao_stdios[ao_num_stdios]._pollchar = _pollchar; - ao_stdios[ao_num_stdios].putchar = putchar; - ao_stdios[ao_num_stdios].flush = flush; - ao_stdios[ao_num_stdios].echo = 1; -#if AO_NUM_STDIOS > 1 - return ao_num_stdios++; -#else - return 0; -#endif -} diff --git a/src/core/ao_storage.c b/src/core/ao_storage.c deleted file mode 100644 index 6eddae7f..00000000 --- a/src/core/ao_storage.c +++ /dev/null @@ -1,186 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include - -uint8_t -ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant -{ - uint16_t this_len; - uint16_t this_off; - - ao_storage_setup(); - if (pos >= ao_storage_total || pos + len > ao_storage_total) - return 0; - while (len) { - - /* Compute portion of transfer within - * a single block - */ - this_off = (uint16_t) pos & (ao_storage_unit - 1); - this_len = ao_storage_unit - this_off; - if (this_len > len) - this_len = len; - - if (!ao_storage_device_read(pos, buf, this_len)) - return 0; - - /* See how much is left */ - buf += this_len; - len -= this_len; - pos += this_len; - } - return 1; -} - -uint8_t -ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant -{ - uint16_t this_len; - uint16_t this_off; - - ao_storage_setup(); - if (pos >= ao_storage_total || pos + len > ao_storage_total) - return 0; - while (len) { - - /* Compute portion of transfer within - * a single block - */ - this_off = (uint16_t) pos & (ao_storage_unit - 1); - this_len = ao_storage_unit - this_off; - if (this_len > len) - this_len = len; - - if (!ao_storage_device_write(pos, buf, this_len)) - return 0; - - /* See how much is left */ - buf += this_len; - len -= this_len; - pos += this_len; - } - return 1; -} - -static __xdata uint8_t storage_data[8]; - -static void -ao_storage_dump(void) __reentrant -{ - uint8_t i, j; - - ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) - return; - for (i = 0; ; i += 8) { - if (ao_storage_read(((uint32_t) (ao_cmd_lex_i) << 8) + i, - storage_data, - 8)) { - ao_cmd_put16((uint16_t) i); - for (j = 0; j < 8; j++) { - putchar(' '); - ao_cmd_put8(storage_data[j]); - } - putchar ('\n'); - } - if (i == 248) - break; - } -} - -#if HAS_STORAGE_DEBUG - -/* not enough space for this today - */ -static void -ao_storage_store(void) __reentrant -{ - uint16_t block; - uint8_t i; - uint16_t len; - static __xdata uint8_t b; - uint32_t addr; - - ao_cmd_hex(); - block = ao_cmd_lex_i; - ao_cmd_hex(); - i = ao_cmd_lex_i; - addr = ((uint32_t) block << 8) | i; - ao_cmd_hex(); - len = ao_cmd_lex_i; - if (ao_cmd_status != ao_cmd_success) - return; - while (len--) { - ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) - return; - b = ao_cmd_lex_i; - ao_storage_write(addr, &b, 1); - addr++; - } -} -#endif - -void -ao_storage_zap(void) __reentrant -{ - ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) - return; - ao_storage_erase((uint32_t) ao_cmd_lex_i << 8); -} - -void -ao_storage_zapall(void) __reentrant -{ - uint32_t pos; - - ao_cmd_white(); - if (!ao_match_word("DoIt")) - return; - for (pos = 0; pos < ao_storage_log_max; pos += ao_storage_block) - ao_storage_erase(pos); -} - -void -ao_storage_info(void) __reentrant -{ - ao_storage_setup(); - printf("Storage size: %ld\n", (long) ao_storage_total); - printf("Storage erase unit: %ld\n", (long) ao_storage_block); - ao_storage_device_info(); -} - -__code struct ao_cmds ao_storage_cmds[] = { - { ao_storage_info, "f\0Show storage" }, - { ao_storage_dump, "e \0Dump flash" }, -#if HAS_STORAGE_DEBUG - { ao_storage_store, "w ...\0Write data to flash" }, -#endif - { ao_storage_zap, "z \0Erase " }, - { ao_storage_zapall,"Z \0Erase all. is doit with D&I" }, - { 0, NULL }, -}; - -void -ao_storage_init(void) -{ - ao_storage_device_init(); - ao_cmd_register(&ao_storage_cmds[0]); -} diff --git a/src/core/ao_storage.h b/src/core/ao_storage.h deleted file mode 100644 index 6cc6fcb7..00000000 --- a/src/core/ao_storage.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_STORAGE_H_ -#define _AO_STORAGE_H_ - -/* - * Storage interface, provided by one of the eeprom or flash - * drivers - */ - -#ifndef ao_storage_pos_t -#define ao_storage_pos_t uint32_t -#endif - -typedef ao_storage_pos_t ao_pos_t; - -/* Total bytes of available storage */ -extern __pdata ao_pos_t ao_storage_total; - -/* Block size - device is erased in these units. At least 256 bytes */ -extern __pdata ao_pos_t ao_storage_block; - -#ifndef USE_STORAGE_CONFIG -#define USE_STORAGE_CONFIG 1 -#endif - -#if USE_STORAGE_CONFIG -/* Byte offset of config block. Will be ao_storage_block bytes long */ -extern __pdata ao_pos_t ao_storage_config; - -#define ao_storage_log_max ao_storage_config -#else -#define ao_storage_log_max ao_storage_total -#endif - -/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */ -extern __pdata uint16_t ao_storage_unit; - -/* Initialize above values. Can only be called once the OS is running */ -void -ao_storage_setup(void) __reentrant; - -/* Write data. Returns 0 on failure, 1 on success */ -uint8_t -ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; - -/* Read data. Returns 0 on failure, 1 on success */ -uint8_t -ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; - -/* Erase a block of storage. This always clears ao_storage_block bytes */ -uint8_t -ao_storage_erase(ao_pos_t pos) __reentrant; - -/* Flush any pending writes to stable storage */ -void -ao_storage_flush(void) __reentrant; - -/* Initialize the storage code */ -void -ao_storage_init(void); - -/* - * Low-level functions wrapped by ao_storage.c - */ - -/* Read data within a storage unit */ -uint8_t -ao_storage_device_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; - -/* Write data within a storage unit */ -uint8_t -ao_storage_device_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; - -/* Initialize low-level device bits */ -void -ao_storage_device_init(void); - -/* Print out information about flash chips */ -void -ao_storage_device_info(void) __reentrant; - -#endif /* _AO_STORAGE_H_ */ diff --git a/src/core/ao_task.c b/src/core/ao_task.c deleted file mode 100644 index bafb4943..00000000 --- a/src/core/ao_task.c +++ /dev/null @@ -1,548 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include -#include -#if HAS_SAMPLE_PROFILE -#include -#endif -#if HAS_STACK_GUARD -#include -#endif - -#define DEBUG 0 - -#define AO_NO_TASK_INDEX 0xff - -__xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; -__data uint8_t ao_num_tasks; -__xdata struct ao_task *__data ao_cur_task; - -#if !HAS_TASK_QUEUE -static __data uint8_t ao_cur_task_index; -#endif - -#ifdef ao_arch_task_globals -ao_arch_task_globals -#endif - -#define AO_CHECK_STACK 0 - -#if AO_CHECK_STACK -static uint8_t in_yield; - -static inline void ao_check_stack(void) { - uint8_t q; - if (!in_yield && ao_cur_task && &q < &ao_cur_task->stack[0]) - ao_panic(AO_PANIC_STACK); -} -#else -#define ao_check_stack() -#endif - -#if HAS_TASK_QUEUE - -#define SLEEP_HASH_SIZE 17 - -static struct ao_list run_queue; -static struct ao_list alarm_queue; -static struct ao_list sleep_queue[SLEEP_HASH_SIZE]; - -static void -ao_task_to_run_queue(struct ao_task *task) -{ - ao_list_del(&task->queue); - ao_list_append(&task->queue, &run_queue); -} - -static struct ao_list * -ao_task_sleep_queue(void *wchan) -{ - return &sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE]; -} - -static void -ao_task_to_sleep_queue(struct ao_task *task, void *wchan) -{ - ao_list_del(&task->queue); - ao_list_append(&task->queue, ao_task_sleep_queue(wchan)); -} - -#if DEBUG -static void -ao_task_validate_alarm_queue(void) -{ - struct ao_task *alarm, *prev = NULL; - int i; - - if (ao_list_is_empty(&alarm_queue)) - return; - ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { - if (prev) { - if ((int16_t) (alarm->alarm - prev->alarm) < 0) { - ao_panic(1); - } - } - prev = alarm; - } - for (i = 0; i < ao_num_tasks; i++) { - alarm = ao_tasks[i]; - if (alarm->alarm) { - if (ao_list_is_empty(&alarm->alarm_queue)) - ao_panic(2); - } else { - if (!ao_list_is_empty(&alarm->alarm_queue)) - ao_panic(3); - } - } - if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm) - ao_panic(4); -} -#else -#define ao_task_validate_alarm_queue() -#endif - -uint16_t ao_task_alarm_tick; - -static void -ao_task_to_alarm_queue(struct ao_task *task) -{ - struct ao_task *alarm; - ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { - if ((int16_t) (alarm->alarm - task->alarm) >= 0) { - ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev); - ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; - ao_task_validate_alarm_queue(); - return; - } - } - ao_list_append(&task->alarm_queue, &alarm_queue); - ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; - ao_task_validate_alarm_queue(); -} - -static void -ao_task_from_alarm_queue(struct ao_task *task) -{ - ao_list_del(&task->alarm_queue); - if (ao_list_is_empty(&alarm_queue)) - ao_task_alarm_tick = 0; - else - ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; - ao_task_validate_alarm_queue(); -} - -static void -ao_task_init_queue(struct ao_task *task) -{ - ao_list_init(&task->queue); - ao_list_init(&task->alarm_queue); -} - -static void -ao_task_exit_queue(struct ao_task *task) -{ - ao_list_del(&task->queue); - ao_list_del(&task->alarm_queue); -} - -void -ao_task_check_alarm(uint16_t tick) -{ - struct ao_task *alarm, *next; - - ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) { - if ((int16_t) (tick - alarm->alarm) < 0) - break; - alarm->alarm = 0; - ao_task_from_alarm_queue(alarm); - ao_task_to_run_queue(alarm); - } -} - -void -ao_task_init(void) -{ - uint8_t i; - ao_list_init(&run_queue); - ao_list_init(&alarm_queue); - ao_task_alarm_tick = 0; - for (i = 0; i < SLEEP_HASH_SIZE; i++) - ao_list_init(&sleep_queue[i]); -} - -#if DEBUG -static uint8_t -ao_task_validate_queue(struct ao_task *task) -{ - uint32_t flags; - struct ao_task *m; - uint8_t ret = 0; - struct ao_list *queue; - - flags = ao_arch_irqsave(); - if (task->wchan) { - queue = ao_task_sleep_queue(task->wchan); - ret |= 2; - } else { - queue = &run_queue; - ret |= 4; - } - ao_list_for_each_entry(m, queue, struct ao_task, queue) { - if (m == task) { - ret |= 1; - break; - } - } - ao_arch_irqrestore(flags); - return ret; -} - -static uint8_t -ao_task_validate_alarm(struct ao_task *task) -{ - uint32_t flags; - struct ao_task *m; - uint8_t ret = 0; - - flags = ao_arch_irqsave(); - if (task->alarm == 0) - return 0xff; - ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) { - if (m == task) - ret |= 1; - else { - if (!(ret&1)) { - if ((int16_t) (m->alarm - task->alarm) > 0) - ret |= 2; - } else { - if ((int16_t) (task->alarm - m->alarm) > 0) - ret |= 4; - } - } - } - ao_arch_irqrestore(flags); - return ret; -} - - -static void -ao_task_validate(void) -{ - uint8_t i; - struct ao_task *task; - uint8_t ret; - - for (i = 0; i < ao_num_tasks; i++) { - task = ao_tasks[i]; - ret = ao_task_validate_queue(task); - if (!(ret & 1)) { - if (ret & 2) - printf ("sleeping task not on sleep queue %s %08x\n", - task->name, task->wchan); - else - printf ("running task not on run queue %s\n", - task->name); - } - ret = ao_task_validate_alarm(task); - if (ret != 0xff) { - if (!(ret & 1)) - printf ("alarm task not on alarm queue %s %d\n", - task->name, task->alarm); - if (ret & 2) - printf ("alarm queue has sooner entries after %s %d\n", - task->name, task->alarm); - if (ret & 4) - printf ("alarm queue has later entries before %s %d\n", - task->name, task->alarm); - } - } -} -#endif /* DEBUG */ - -#endif /* HAS_TASK_QUEUE */ - -void -ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant -{ - uint8_t task_id; - uint8_t t; - if (ao_num_tasks == AO_NUM_TASKS) - ao_panic(AO_PANIC_NO_TASK); - for (task_id = 1; task_id != 0; task_id++) { - for (t = 0; t < ao_num_tasks; t++) - if (ao_tasks[t]->task_id == task_id) - break; - if (t == ao_num_tasks) - break; - } - task->task_id = task_id; - task->name = name; - task->wchan = NULL; - /* - * Construct a stack frame so that it will 'return' - * to the start of the task - */ - ao_arch_init_stack(task, start); - ao_arch_critical( -#if HAS_TASK_QUEUE - ao_task_init_queue(task); - ao_task_to_run_queue(task); -#endif - ao_tasks[ao_num_tasks] = task; - ao_num_tasks++; - ); -} - -__data uint8_t ao_task_minimize_latency; - -/* Task switching function. This must not use any stack variables */ -void -ao_yield(void) ao_arch_naked_define -{ - ao_arch_save_regs(); - -#if HAS_TASK_QUEUE - if (ao_cur_task == NULL) - ao_cur_task = ao_tasks[ao_num_tasks-1]; -#else - if (ao_cur_task_index == AO_NO_TASK_INDEX) - ao_cur_task_index = ao_num_tasks-1; -#endif - else - { -#if HAS_SAMPLE_PROFILE - uint16_t tick = ao_sample_profile_timer_value(); - uint16_t run = tick - ao_cur_task->start; - if (run > ao_cur_task->max_run) - ao_cur_task->max_run = run; - ++ao_cur_task->yields; -#endif - ao_arch_save_stack(); - } - - ao_arch_isr_stack(); -#if !HAS_TASK_QUEUE - if (ao_task_minimize_latency) - ao_arch_release_interrupts(); - else -#endif - ao_arch_block_interrupts(); - -#if AO_CHECK_STACK - in_yield = 1; -#endif - /* Find a task to run. If there isn't any runnable task, - * this loop will run forever, which is just fine - */ -#if HAS_TASK_QUEUE - /* If the current task is running, move it to the - * end of the queue to allow other tasks a chance - */ - if (ao_cur_task->wchan == NULL) - ao_task_to_run_queue(ao_cur_task); - ao_cur_task = NULL; - for (;;) { - ao_arch_memory_barrier(); - if (!ao_list_is_empty(&run_queue)) - break; - /* Wait for interrupts when there's nothing ready */ - ao_arch_wait_interrupt(); - } - ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue); -#else - { - __pdata uint8_t ao_last_task_index = ao_cur_task_index; - for (;;) { - ++ao_cur_task_index; - if (ao_cur_task_index == ao_num_tasks) - ao_cur_task_index = 0; - - ao_cur_task = ao_tasks[ao_cur_task_index]; - - /* Check for ready task */ - if (ao_cur_task->wchan == NULL) - break; - - /* Check if the alarm is set for a time which has passed */ - if (ao_cur_task->alarm && - (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) - break; - - /* Wait for interrupts when there's nothing ready */ - if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency) - ao_arch_wait_interrupt(); - } - } -#endif -#if HAS_SAMPLE_PROFILE - ao_cur_task->start = ao_sample_profile_timer_value(); -#endif -#if HAS_STACK_GUARD - ao_mpu_stack_guard(ao_cur_task->stack); -#endif -#if AO_CHECK_STACK - in_yield = 0; -#endif - ao_arch_restore_stack(); -} - -uint8_t -ao_sleep(__xdata void *wchan) -{ -#if HAS_TASK_QUEUE - uint32_t flags; - flags = ao_arch_irqsave(); -#endif - ao_cur_task->wchan = wchan; -#if HAS_TASK_QUEUE - ao_task_to_sleep_queue(ao_cur_task, wchan); - ao_arch_irqrestore(flags); -#endif - ao_yield(); - if (ao_cur_task->wchan) { - ao_cur_task->wchan = NULL; - ao_cur_task->alarm = 0; - return 1; - } - return 0; -} - -void -ao_wakeup(__xdata void *wchan) __reentrant -{ -#if HAS_TASK_QUEUE - struct ao_task *sleep, *next; - struct ao_list *sleep_queue; - uint32_t flags; - - if (ao_num_tasks == 0) - return; - sleep_queue = ao_task_sleep_queue(wchan); - flags = ao_arch_irqsave(); - ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) { - if (sleep->wchan == wchan) { - sleep->wchan = NULL; - ao_task_to_run_queue(sleep); - } - } - ao_arch_irqrestore(flags); -#else - uint8_t i; - for (i = 0; i < ao_num_tasks; i++) - if (ao_tasks[i]->wchan == wchan) - ao_tasks[i]->wchan = NULL; -#endif - ao_check_stack(); -} - -void -ao_alarm(uint16_t delay) -{ -#if HAS_TASK_QUEUE - uint32_t flags; - /* Make sure we sleep *at least* delay ticks, which means adding - * one to account for the fact that we may be close to the next tick - */ - flags = ao_arch_irqsave(); -#endif - if (!(ao_cur_task->alarm = ao_time() + delay + 1)) - ao_cur_task->alarm = 1; -#if HAS_TASK_QUEUE - ao_task_to_alarm_queue(ao_cur_task); - ao_arch_irqrestore(flags); -#endif -} - -void -ao_clear_alarm(void) -{ -#if HAS_TASK_QUEUE - uint32_t flags; - - flags = ao_arch_irqsave(); -#endif - ao_cur_task->alarm = 0; -#if HAS_TASK_QUEUE - ao_task_from_alarm_queue(ao_cur_task); - ao_arch_irqrestore(flags); -#endif -} - -static __xdata uint8_t ao_forever; - -void -ao_delay(uint16_t ticks) -{ - ao_alarm(ticks); - ao_sleep(&ao_forever); - ao_clear_alarm(); -} - -void -ao_exit(void) -{ - uint8_t i; - ao_arch_block_interrupts(); - ao_num_tasks--; -#if HAS_TASK_QUEUE - for (i = 0; i < ao_num_tasks; i++) - if (ao_tasks[i] == ao_cur_task) - break; - ao_task_exit_queue(ao_cur_task); -#else - i = ao_cur_task_index; - ao_cur_task_index = AO_NO_TASK_INDEX; -#endif - for (; i < ao_num_tasks; i++) - ao_tasks[i] = ao_tasks[i+1]; - ao_cur_task = NULL; - ao_yield(); - /* we'll never get back here */ -} - -#if HAS_TASK_INFO -void -ao_task_info(void) -{ - uint8_t i; - __xdata struct ao_task *task; - - for (i = 0; i < ao_num_tasks; i++) { - task = ao_tasks[i]; - printf("%12s: wchan %04x\n", - task->name, - (int) task->wchan); - } -#if HAS_TASK_QUEUE && DEBUG - ao_task_validate(); -#endif -} -#endif - -void -ao_start_scheduler(void) -{ -#if !HAS_TASK_QUEUE - ao_cur_task_index = AO_NO_TASK_INDEX; -#endif - ao_cur_task = NULL; -#if HAS_ARCH_START_SCHEDULER - ao_arch_start_scheduler(); -#endif - ao_yield(); -} diff --git a/src/core/ao_task.h b/src/core/ao_task.h deleted file mode 100644 index 9c56b480..00000000 --- a/src/core/ao_task.h +++ /dev/null @@ -1,117 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_TASK_H_ -#define _AO_TASK_H_ -#if HAS_TASK_QUEUE -#include -#endif - -#ifndef HAS_TASK_INFO -#define HAS_TASK_INFO 1 -#endif - -/* An AltOS task */ -struct ao_task { - __xdata void *wchan; /* current wait channel (NULL if running) */ - uint16_t alarm; /* abort ao_sleep time */ - ao_arch_task_members /* any architecture-specific fields */ - uint8_t task_id; /* unique id */ - __code char *name; /* task name */ -#if HAS_TASK_QUEUE - struct ao_list queue; - struct ao_list alarm_queue; -#endif - uint8_t stack[AO_STACK_SIZE]; /* saved stack */ -#if HAS_SAMPLE_PROFILE - uint32_t ticks; - uint32_t yields; - uint16_t start; - uint16_t max_run; -#endif -}; - -#ifndef AO_NUM_TASKS -#define AO_NUM_TASKS 16 /* maximum number of tasks */ -#endif - -#define AO_NO_TASK 0 /* no task id */ - -extern __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; -extern __data uint8_t ao_num_tasks; -extern __xdata struct ao_task *__data ao_cur_task; -extern __data uint8_t ao_task_minimize_latency; /* Reduce IRQ latency */ - -/* - ao_task.c - */ - -/* Suspend the current task until wchan is awoken. - * returns: - * 0 on normal wake - * 1 on alarm - */ -uint8_t -ao_sleep(__xdata void *wchan); - -/* Wake all tasks sleeping on wchan */ -void -ao_wakeup(__xdata void *wchan) __reentrant; - -/* set an alarm to go off in 'delay' ticks */ -void -ao_alarm(uint16_t delay); - -/* Clear any pending alarm */ -void -ao_clear_alarm(void); - -/* Yield the processor to another task */ -void -ao_yield(void) ao_arch_naked_declare; - -/* Add a task to the run queue */ -void -ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant; - -#if HAS_TASK_QUEUE -/* Called on timer interrupt to check alarms */ -extern uint16_t ao_task_alarm_tick; -void -ao_task_check_alarm(uint16_t tick); -#endif - -/* Terminate the current task */ -void -ao_exit(void); - -/* Dump task info to console */ -void -ao_task_info(void); - -/* Start the scheduler. This will not return */ -void -ao_start_scheduler(void); - -#if HAS_TASK_QUEUE -void -ao_task_init(void); -#else -#define ao_task_init() -#endif - -#endif diff --git a/src/core/ao_telem.h b/src/core/ao_telem.h deleted file mode 100644 index 1a8da291..00000000 --- a/src/core/ao_telem.h +++ /dev/null @@ -1,172 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_TELEM_H_ -#define _AO_TELEM_H_ - -#define AO_TELEMETRY_VERSION 4 - -/* - * Telemetry version 4 and higher format: - * - * General header fields - * - * Name Value - * - * VERSION Telemetry version number (4 or more). Must be first. - * c Callsign (string, no spaces allowed) - * n Flight unit serial number (integer) - * f Flight number (integer) - * r Packet RSSI value (integer) - * s Flight computer state (string, no spaces allowed) - * t Flight computer clock (integer in centiseconds) - */ - -#define AO_TELEM_VERSION "VERSION" -#define AO_TELEM_CALL "c" -#define AO_TELEM_SERIAL "n" -#define AO_TELEM_FLIGHT "f" -#define AO_TELEM_RSSI "r" -#define AO_TELEM_STATE "s" -#define AO_TELEM_TICK "t" - -/* - * Raw sensor values - * - * Name Value - * r_a Accelerometer reading (integer) - * r_b Barometer reading (integer) - * r_t Thermometer reading (integer) - * r_v Battery reading (integer) - * r_d Drogue continuity (integer) - * r_m Main continuity (integer) - */ - -#define AO_TELEM_RAW_ACCEL "r_a" -#define AO_TELEM_RAW_BARO "r_b" -#define AO_TELEM_RAW_THERMO "r_t" -#define AO_TELEM_RAW_BATT "r_v" -#define AO_TELEM_RAW_DROGUE "r_d" -#define AO_TELEM_RAW_MAIN "r_m" - -/* - * Sensor calibration values - * - * Name Value - * c_a Ground accelerometer reading (integer) - * c_b Ground barometer reading (integer) - * c_p Accelerometer reading for +1g - * c_m Accelerometer reading for -1g - */ - -#define AO_TELEM_CAL_ACCEL_GROUND "c_a" -#define AO_TELEM_CAL_BARO_GROUND "c_b" -#define AO_TELEM_CAL_ACCEL_PLUS "c_p" -#define AO_TELEM_CAL_ACCEL_MINUS "c_m" - -/* - * Kalman state values - * - * Name Value - * k_h Height above pad (integer, meters) - * k_s Vertical speeed (integer, m/s * 16) - * k_a Vertical acceleration (integer, m/s² * 16) - */ - -#define AO_TELEM_KALMAN_HEIGHT "k_h" -#define AO_TELEM_KALMAN_SPEED "k_s" -#define AO_TELEM_KALMAN_ACCEL "k_a" - -/* - * Ad-hoc flight values - * - * Name Value - * a_a Acceleration (integer, sensor units) - * a_s Speed (integer, integrated acceleration value) - * a_b Barometer reading (integer, sensor units) - */ - -#define AO_TELEM_ADHOC_ACCEL "a_a" -#define AO_TELEM_ADHOC_SPEED "a_s" -#define AO_TELEM_ADHOC_BARO "a_b" - -/* - * GPS values - * - * Name Value - * g GPS state (string): - * l locked - * u unlocked - * e error (missing or broken) - * g_n Number of sats used in solution - * g_ns Latitude (degrees * 10e7) - * g_ew Longitude (degrees * 10e7) - * g_a Altitude (integer meters) - * g_Y GPS year (integer) - * g_M GPS month (integer - 1-12) - * g_D GPS day (integer - 1-31) - * g_h GPS hour (integer - 0-23) - * g_m GPS minute (integer - 0-59) - * g_s GPS second (integer - 0-59) - * g_v GPS vertical speed (integer, cm/sec) - * g_g GPS horizontal speed (integer, cm/sec) - * g_c GPS course (integer, 0-359) - * g_hd GPS hdop (integer * 10) - * g_vd GPS vdop (integer * 10) - * g_he GPS h error (integer) - * g_ve GPS v error (integer) - */ - -#define AO_TELEM_GPS_STATE "g" -#define AO_TELEM_GPS_STATE_LOCKED 'l' -#define AO_TELEM_GPS_STATE_UNLOCKED 'u' -#define AO_TELEM_GPS_STATE_ERROR 'e' -#define AO_TELEM_GPS_NUM_SAT "g_n" -#define AO_TELEM_GPS_LATITUDE "g_ns" -#define AO_TELEM_GPS_LONGITUDE "g_ew" -#define AO_TELEM_GPS_ALTITUDE "g_a" -#define AO_TELEM_GPS_YEAR "g_Y" -#define AO_TELEM_GPS_MONTH "g_M" -#define AO_TELEM_GPS_DAY "g_D" -#define AO_TELEM_GPS_HOUR "g_h" -#define AO_TELEM_GPS_MINUTE "g_m" -#define AO_TELEM_GPS_SECOND "g_s" -#define AO_TELEM_GPS_VERTICAL_SPEED "g_v" -#define AO_TELEM_GPS_HORIZONTAL_SPEED "g_g" -#define AO_TELEM_GPS_COURSE "g_c" -#define AO_TELEM_GPS_HDOP "g_hd" -#define AO_TELEM_GPS_VDOP "g_vd" -#define AO_TELEM_GPS_HERROR "g_he" -#define AO_TELEM_GPS_VERROR "g_ve" - -/* - * GPS satellite values - * - * Name Value - * s_n Number of satellites reported (integer) - * s_v0 Space vehicle ID (integer) for report 0 - * s_c0 C/N0 number (integer) for report 0 - * s_v1 Space vehicle ID (integer) for report 1 - * s_c1 C/N0 number (integer) for report 1 - * ... - */ - -#define AO_TELEM_SAT_NUM "s_n" -#define AO_TELEM_SAT_SVID "s_v" -#define AO_TELEM_SAT_C_N_0 "s_c" - -#endif /* _AO_TELEM_H_ */ diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c deleted file mode 100644 index a1c19185..00000000 --- a/src/core/ao_telemetry.c +++ /dev/null @@ -1,555 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#include "ao.h" -#include "ao_log.h" -#include "ao_product.h" - -#ifndef HAS_RDF -#define HAS_RDF 1 -#endif - -static __pdata uint16_t ao_telemetry_interval; - -#if HAS_RDF -static __pdata uint8_t ao_rdf = 0; -static __pdata uint16_t ao_rdf_time; -#endif - -#if HAS_APRS -static __pdata uint16_t ao_aprs_time; - -#include -#endif - -#if defined(TELEMEGA) -#define AO_SEND_MEGA 1 -#endif - -#if defined (TELEMETRUM_V_2_0) -#define AO_SEND_METRUM 1 -#endif - -#if defined(TELEMETRUM_V_0_1) || defined(TELEMETRUM_V_0_2) || defined(TELEMETRUM_V_1_0) || defined(TELEMETRUM_V_1_1) || defined(TELEBALLOON_V_1_1) || defined(TELEMETRUM_V_1_2) -#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMETRUM -#endif - -#if defined(TELEMINI_V_1_0) -#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMINI -#endif - -#if defined(TELENANO_V_0_1) -#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELENANO -#endif - -static __xdata union ao_telemetry_all telemetry; - -#if defined AO_TELEMETRY_SENSOR -/* Send sensor packet */ -static void -ao_send_sensor(void) -{ - __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - - telemetry.generic.tick = packet->tick; - telemetry.generic.type = AO_TELEMETRY_SENSOR; - - telemetry.sensor.state = ao_flight_state; -#if HAS_ACCEL - telemetry.sensor.accel = packet->adc.accel; -#else - telemetry.sensor.accel = 0; -#endif - telemetry.sensor.pres = ao_data_pres(packet); - telemetry.sensor.temp = packet->adc.temp; - telemetry.sensor.v_batt = packet->adc.v_batt; -#if HAS_IGNITE - telemetry.sensor.sense_d = packet->adc.sense_d; - telemetry.sensor.sense_m = packet->adc.sense_m; -#else - telemetry.sensor.sense_d = 0; - telemetry.sensor.sense_m = 0; -#endif - - telemetry.sensor.acceleration = ao_accel; - telemetry.sensor.speed = ao_speed; - telemetry.sensor.height = ao_height; - - telemetry.sensor.ground_pres = ao_ground_pres; -#if HAS_ACCEL - telemetry.sensor.ground_accel = ao_ground_accel; - telemetry.sensor.accel_plus_g = ao_config.accel_plus_g; - telemetry.sensor.accel_minus_g = ao_config.accel_minus_g; -#else - telemetry.sensor.ground_accel = 0; - telemetry.sensor.accel_plus_g = 0; - telemetry.sensor.accel_minus_g = 0; -#endif - - ao_radio_send(&telemetry, sizeof (telemetry)); -} -#endif - - -#ifdef AO_SEND_MEGA -/* Send mega sensor packet */ -static void -ao_send_mega_sensor(void) -{ - __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - - telemetry.generic.tick = packet->tick; - telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; - - telemetry.mega_sensor.orient = ao_sample_orient; - telemetry.mega_sensor.accel = ao_data_accel(packet); - telemetry.mega_sensor.pres = ao_data_pres(packet); - telemetry.mega_sensor.temp = ao_data_temp(packet); - -#if HAS_MPU6000 - telemetry.mega_sensor.accel_x = packet->mpu6000.accel_x; - telemetry.mega_sensor.accel_y = packet->mpu6000.accel_y; - telemetry.mega_sensor.accel_z = packet->mpu6000.accel_z; - - telemetry.mega_sensor.gyro_x = packet->mpu6000.gyro_x; - telemetry.mega_sensor.gyro_y = packet->mpu6000.gyro_y; - telemetry.mega_sensor.gyro_z = packet->mpu6000.gyro_z; -#endif - -#if HAS_HMC5883 - telemetry.mega_sensor.mag_x = packet->hmc5883.x; - telemetry.mega_sensor.mag_y = packet->hmc5883.y; - telemetry.mega_sensor.mag_z = packet->hmc5883.z; -#endif - - ao_radio_send(&telemetry, sizeof (telemetry)); -} - -static __pdata int8_t ao_telemetry_mega_data_max; -static __pdata int8_t ao_telemetry_mega_data_cur; - -/* Send mega data packet */ -static void -ao_send_mega_data(void) -{ - if (--ao_telemetry_mega_data_cur <= 0) { - __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - uint8_t i; - - telemetry.generic.tick = packet->tick; - telemetry.generic.type = AO_TELEMETRY_MEGA_DATA; - - telemetry.mega_data.state = ao_flight_state; - telemetry.mega_data.v_batt = packet->adc.v_batt; - telemetry.mega_data.v_pyro = packet->adc.v_pbatt; - - /* ADC range is 0-4095, so shift by four to save the high 8 bits */ - for (i = 0; i < AO_ADC_NUM_SENSE; i++) - telemetry.mega_data.sense[i] = packet->adc.sense[i] >> 4; - - telemetry.mega_data.ground_pres = ao_ground_pres; - telemetry.mega_data.ground_accel = ao_ground_accel; - telemetry.mega_data.accel_plus_g = ao_config.accel_plus_g; - telemetry.mega_data.accel_minus_g = ao_config.accel_minus_g; - - telemetry.mega_data.acceleration = ao_accel; - telemetry.mega_data.speed = ao_speed; - telemetry.mega_data.height = ao_height; - - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_mega_data_cur = ao_telemetry_mega_data_max; - } -} -#endif /* AO_SEND_MEGA */ - -#ifdef AO_SEND_METRUM -/* Send telemetrum sensor packet */ -static void -ao_send_metrum_sensor(void) -{ - __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - - telemetry.generic.tick = packet->tick; - telemetry.generic.type = AO_TELEMETRY_METRUM_SENSOR; - - telemetry.metrum_sensor.state = ao_flight_state; -#if HAS_ACCEL - telemetry.metrum_sensor.accel = ao_data_accel(packet); -#endif - telemetry.metrum_sensor.pres = ao_data_pres(packet); - telemetry.metrum_sensor.temp = ao_data_temp(packet); - - telemetry.metrum_sensor.acceleration = ao_accel; - telemetry.metrum_sensor.speed = ao_speed; - telemetry.metrum_sensor.height = ao_height; - - telemetry.metrum_sensor.v_batt = packet->adc.v_batt; - telemetry.metrum_sensor.sense_a = packet->adc.sense_a; - telemetry.metrum_sensor.sense_m = packet->adc.sense_m; - - ao_radio_send(&telemetry, sizeof (telemetry)); -} - -static __pdata int8_t ao_telemetry_metrum_data_max; -static __pdata int8_t ao_telemetry_metrum_data_cur; - -/* Send telemetrum data packet */ -static void -ao_send_metrum_data(void) -{ - if (--ao_telemetry_metrum_data_cur <= 0) { - __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - - telemetry.generic.tick = packet->tick; - telemetry.generic.type = AO_TELEMETRY_METRUM_DATA; - - telemetry.metrum_data.ground_pres = ao_ground_pres; -#if HAS_ACCEL - telemetry.metrum_data.ground_accel = ao_ground_accel; - telemetry.metrum_data.accel_plus_g = ao_config.accel_plus_g; - telemetry.metrum_data.accel_minus_g = ao_config.accel_minus_g; -#else - telemetry.metrum_data.ground_accel = 1; - telemetry.metrum_data.accel_plus_g = 0; - telemetry.metrum_data.accel_minus_g = 2; -#endif - - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_metrum_data_cur = ao_telemetry_metrum_data_max; - } -} -#endif /* AO_SEND_METRUM */ - -#ifdef AO_SEND_MINI - -static void -ao_send_mini(void) -{ - __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - - telemetry.generic.tick = packet->tick; - telemetry.generic.type = AO_TELEMETRY_MINI; - - telemetry.mini.state = ao_flight_state; - - telemetry.mini.v_batt = packet->adc.v_batt; - telemetry.mini.sense_a = packet->adc.sense_a; - telemetry.mini.sense_m = packet->adc.sense_m; - - telemetry.mini.pres = ao_data_pres(packet); - telemetry.mini.temp = ao_data_temp(packet); - - telemetry.mini.acceleration = ao_accel; - telemetry.mini.speed = ao_speed; - telemetry.mini.height = ao_height; - - telemetry.mini.ground_pres = ao_ground_pres; - - ao_radio_send(&telemetry, sizeof (telemetry)); -} - -#endif /* AO_SEND_MINI */ - -#ifdef AO_SEND_ALL_BARO -static uint8_t ao_baro_sample; - -static void -ao_send_baro(void) -{ - uint8_t sample = ao_sample_data; - uint8_t samples = (sample - ao_baro_sample) & (AO_DATA_RING - 1); - - if (samples > 12) { - ao_baro_sample = (ao_baro_sample + (samples - 12)) & (AO_DATA_RING - 1); - samples = 12; - } - telemetry.generic.tick = ao_data_ring[sample].tick; - telemetry.generic.type = AO_TELEMETRY_BARO; - telemetry.baro.samples = samples; - for (sample = 0; sample < samples; sample++) { - telemetry.baro.baro[sample] = ao_data_ring[ao_baro_sample].adc.pres; - ao_baro_sample = ao_data_ring_next(ao_baro_sample); - } - ao_radio_send(&telemetry, sizeof (telemetry)); -} -#endif - -static __pdata int8_t ao_telemetry_config_max; -static __pdata int8_t ao_telemetry_config_cur; - -static void -ao_send_configuration(void) -{ - if (--ao_telemetry_config_cur <= 0) - { - telemetry.generic.type = AO_TELEMETRY_CONFIGURATION; - telemetry.configuration.device = AO_idProduct_NUMBER; -#if HAS_LOG - telemetry.configuration.flight = ao_log_full() ? 0 : ao_flight_number; -#else - telemetry.configuration.flight = ao_flight_number; -#endif - telemetry.configuration.config_major = AO_CONFIG_MAJOR; - telemetry.configuration.config_minor = AO_CONFIG_MINOR; - telemetry.configuration.apogee_delay = ao_config.apogee_delay; - telemetry.configuration.main_deploy = ao_config.main_deploy; - telemetry.configuration.flight_log_max = ao_config.flight_log_max >> 10; - ao_xmemcpy (telemetry.configuration.callsign, - ao_config.callsign, - AO_MAX_CALLSIGN); - ao_xmemcpy (telemetry.configuration.version, - CODE_TO_XDATA(ao_version), - AO_MAX_VERSION); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_config_cur = ao_telemetry_config_max; - } -} - -#if HAS_GPS - -static __pdata int8_t ao_telemetry_loc_cur; -static __pdata int8_t ao_telemetry_sat_cur; - -static void -ao_send_location(void) -{ - if (--ao_telemetry_loc_cur <= 0) - { - telemetry.generic.type = AO_TELEMETRY_LOCATION; - ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&telemetry.location.flags, - &ao_gps_data.flags, - 26); - telemetry.location.tick = ao_gps_tick; - ao_mutex_put(&ao_gps_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_loc_cur = ao_telemetry_config_max; - } -} - -static void -ao_send_satellite(void) -{ - if (--ao_telemetry_sat_cur <= 0) - { - telemetry.generic.type = AO_TELEMETRY_SATELLITE; - ao_mutex_get(&ao_gps_mutex); - telemetry.satellite.channels = ao_gps_tracking_data.channels; - ao_xmemcpy(&telemetry.satellite.sats, - &ao_gps_tracking_data.sats, - AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info)); - ao_mutex_put(&ao_gps_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_sat_cur = ao_telemetry_config_max; - } -} -#endif - -#if HAS_COMPANION - -static __pdata int8_t ao_telemetry_companion_max; -static __pdata int8_t ao_telemetry_companion_cur; - -static void -ao_send_companion(void) -{ - if (--ao_telemetry_companion_cur <= 0) { - telemetry.generic.type = AO_TELEMETRY_COMPANION; - telemetry.companion.board_id = ao_companion_setup.board_id; - telemetry.companion.update_period = ao_companion_setup.update_period; - telemetry.companion.channels = ao_companion_setup.channels; - ao_mutex_get(&ao_companion_mutex); - ao_xmemcpy(&telemetry.companion.companion_data, - ao_companion_data, - ao_companion_setup.channels * 2); - ao_mutex_put(&ao_companion_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_companion_cur = ao_telemetry_companion_max; - } -} -#endif - -void -ao_telemetry(void) -{ - uint16_t time; - int16_t delay; - - ao_config_get(); - if (!ao_config.radio_enable) - ao_exit(); - while (!ao_flight_number) - ao_sleep(&ao_flight_number); - - telemetry.generic.serial = ao_serial_number; - for (;;) { - while (ao_telemetry_interval == 0) - ao_sleep(&telemetry); - time = ao_time(); -#if HAS_RDF - ao_rdf_time = time; -#endif -#if HAS_APRS - ao_aprs_time = time; -#endif - while (ao_telemetry_interval) { -#if HAS_APRS - if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) -#endif - { -#ifdef AO_SEND_ALL_BARO - ao_send_baro(); -#endif - -#if HAS_FLIGHT -# ifdef AO_SEND_MEGA - ao_send_mega_sensor(); - ao_send_mega_data(); -# endif -# ifdef AO_SEND_METRUM - ao_send_metrum_sensor(); - ao_send_metrum_data(); -# endif -# ifdef AO_SEND_MINI - ao_send_mini(); -# endif -# ifdef AO_TELEMETRY_SENSOR - ao_send_sensor(); -# endif -#endif /* HAS_FLIGHT */ - -#if HAS_COMPANION - if (ao_companion_running) - ao_send_companion(); -#endif - ao_send_configuration(); -#if HAS_GPS - ao_send_location(); - ao_send_satellite(); -#endif - } -#ifndef AO_SEND_ALL_BARO -#if HAS_RDF - if (ao_rdf && -#if HAS_APRS - !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && -#endif /* HAS_APRS */ - (int16_t) (ao_time() - ao_rdf_time) >= 0) - { -#if HAS_IGNITE_REPORT - uint8_t c; -#endif /* HAS_IGNITE_REPORT */ - ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; -#if HAS_IGNITE_REPORT - if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) - ao_radio_continuity(c); - else -#endif /* HAS_IGNITE_REPORT*/ - ao_radio_rdf(); - } -#endif /* HAS_RDF */ -#if HAS_APRS - if (ao_config.aprs_interval != 0 && - (int16_t) (ao_time() - ao_aprs_time) >= 0) - { - ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); - ao_aprs_send(); - } -#endif /* HAS_APRS */ -#endif /* !AO_SEND_ALL_BARO */ - time += ao_telemetry_interval; - delay = time - ao_time(); - if (delay > 0) { - ao_alarm(delay); - ao_sleep(&telemetry); - ao_clear_alarm(); - } - else - time = ao_time(); - } - } -} - -void -ao_telemetry_set_interval(uint16_t interval) -{ - int8_t cur = 0; - ao_telemetry_interval = interval; - -#if AO_SEND_MEGA - if (interval > 1) - ao_telemetry_mega_data_max = 1; - else - ao_telemetry_mega_data_max = 2; - if (ao_telemetry_mega_data_max > cur) - cur++; - ao_telemetry_mega_data_cur = cur; -#endif -#if AO_SEND_METRUM - ao_telemetry_metrum_data_max = AO_SEC_TO_TICKS(1) / interval; - if (ao_telemetry_metrum_data_max > cur) - cur++; - ao_telemetry_metrum_data_cur = cur; -#endif - -#if HAS_COMPANION - if (!ao_companion_setup.update_period) - ao_companion_setup.update_period = AO_SEC_TO_TICKS(1); - ao_telemetry_companion_max = ao_companion_setup.update_period / interval; - if (ao_telemetry_companion_max > cur) - cur++; - ao_telemetry_companion_cur = cur; -#endif - - ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval; -#if HAS_COMPANION - if (ao_telemetry_config_max > cur) - cur++; - ao_telemetry_config_cur = cur; -#endif - -#if HAS_GPS - if (ao_telemetry_config_max > cur) - cur++; - ao_telemetry_loc_cur = cur; - if (ao_telemetry_config_max > cur) - cur++; - ao_telemetry_sat_cur = cur; -#endif - ao_wakeup(&telemetry); -} - -#if HAS_RDF -void -ao_rdf_set(uint8_t rdf) -{ - ao_rdf = rdf; - if (rdf == 0) - ao_radio_rdf_abort(); - else { - ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; - } -} -#endif - -__xdata struct ao_task ao_telemetry_task; - -void -ao_telemetry_init() -{ - ao_add_task(&ao_telemetry_task, ao_telemetry, "telemetry"); -} diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h deleted file mode 100644 index 237a35ab..00000000 --- a/src/core/ao_telemetry.h +++ /dev/null @@ -1,321 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_TELEMETRY_H_ -#define _AO_TELEMETRY_H_ - -/* - * ao_telemetry.c - */ -#define AO_MAX_CALLSIGN 8 -#define AO_MAX_VERSION 8 -#if LEGACY_MONITOR -#define AO_MAX_TELEMETRY 128 -#else -#define AO_MAX_TELEMETRY 32 -#endif - -struct ao_telemetry_generic { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t payload[27]; /* 5 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01 -#define AO_TELEMETRY_SENSOR_TELEMINI 0x02 -#define AO_TELEMETRY_SENSOR_TELENANO 0x03 - -struct ao_telemetry_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t accel; /* 6 accelerometer (TM only) */ - int16_t pres; /* 8 pressure sensor */ - int16_t temp; /* 10 temperature sensor */ - int16_t v_batt; /* 12 battery voltage */ - int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */ - int16_t sense_m; /* 16 main continuity sense (TM/Tm) */ - - int16_t acceleration; /* 18 m/s² * 16 */ - int16_t speed; /* 20 m/s * 16 */ - int16_t height; /* 22 m */ - - int16_t ground_pres; /* 24 average pres on pad */ - int16_t ground_accel; /* 26 average accel on pad */ - int16_t accel_plus_g; /* 28 accel calibration at +1g */ - int16_t accel_minus_g; /* 30 accel calibration at -1g */ - /* 32 */ -}; - -#define AO_TELEMETRY_CONFIGURATION 0x04 - -struct ao_telemetry_configuration { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t device; /* 5 device type */ - uint16_t flight; /* 6 flight number */ - uint8_t config_major; /* 8 Config major version */ - uint8_t config_minor; /* 9 Config minor version */ - uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */ - uint16_t main_deploy; /* 12 Main deploy alt in meters */ - uint16_t flight_log_max; /* 14 Maximum flight log size in kB */ - char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */ - char version[AO_MAX_VERSION]; /* 24 Software version */ - /* 32 */ -}; - -#define AO_TELEMETRY_LOCATION 0x05 - -#define AO_GPS_MODE_NOT_VALID 'N' -#define AO_GPS_MODE_AUTONOMOUS 'A' -#define AO_GPS_MODE_DIFFERENTIAL 'D' -#define AO_GPS_MODE_ESTIMATED 'E' -#define AO_GPS_MODE_MANUAL 'M' -#define AO_GPS_MODE_SIMULATED 'S' - -struct ao_telemetry_location { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t flags; /* 5 Number of sats and other flags */ - int16_t altitude; /* 6 GPS reported altitude (m) */ - int32_t latitude; /* 8 latitude (degrees * 10⁷) */ - int32_t longitude; /* 12 longitude (degrees * 10⁷) */ - uint8_t year; /* 16 (- 2000) */ - uint8_t month; /* 17 (1-12) */ - uint8_t day; /* 18 (1-31) */ - uint8_t hour; /* 19 (0-23) */ - uint8_t minute; /* 20 (0-59) */ - uint8_t second; /* 21 (0-59) */ - uint8_t pdop; /* 22 (m * 5) */ - uint8_t hdop; /* 23 (m * 5) */ - uint8_t vdop; /* 24 (m * 5) */ - uint8_t mode; /* 25 */ - uint16_t ground_speed; /* 26 cm/s */ - int16_t climb_rate; /* 28 cm/s */ - uint8_t course; /* 30 degrees / 2 */ - uint8_t unused[1]; /* 31 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SATELLITE 0x06 - -struct ao_telemetry_satellite_info { - uint8_t svid; - uint8_t c_n_1; -}; - -#define AO_TELEMETRY_SATELLITE_MAX_SAT 12 - -struct ao_telemetry_satellite { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t channels; /* 5 number of reported sats */ - - struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT]; /* 6 */ - uint8_t unused[2]; /* 30 */ - /* 32 */ -}; - -#define AO_TELEMETRY_COMPANION 0x07 - -#define AO_COMPANION_MAX_CHANNELS 12 - -struct ao_telemetry_companion { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t board_id; /* 5 */ - - uint8_t update_period; /* 6 */ - uint8_t channels; /* 7 */ - uint16_t companion_data[AO_COMPANION_MAX_CHANNELS]; /* 8 */ - /* 32 */ -}; - -#define AO_TELEMETRY_MEGA_SENSOR 0x08 - -struct ao_telemetry_mega_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t orient; /* 5 angle from vertical */ - int16_t accel; /* 6 Z axis */ - - int32_t pres; /* 8 Pa * 10 */ - int16_t temp; /* 12 °C * 100 */ - - int16_t accel_x; /* 14 */ - int16_t accel_y; /* 16 */ - int16_t accel_z; /* 18 */ - - int16_t gyro_x; /* 20 */ - int16_t gyro_y; /* 22 */ - int16_t gyro_z; /* 24 */ - - int16_t mag_x; /* 26 */ - int16_t mag_y; /* 28 */ - int16_t mag_z; /* 30 */ - /* 32 */ -}; - -#define AO_TELEMETRY_MEGA_DATA 0x09 - -struct ao_telemetry_mega_data { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - - int16_t v_batt; /* 6 battery voltage */ - int16_t v_pyro; /* 8 pyro battery voltage */ - int8_t sense[6]; /* 10 continuity sense */ - - int32_t ground_pres; /* 16 average pres on pad */ - int16_t ground_accel; /* 20 average accel on pad */ - int16_t accel_plus_g; /* 22 accel calibration at +1g */ - int16_t accel_minus_g; /* 24 accel calibration at -1g */ - - int16_t acceleration; /* 26 m/s² * 16 */ - int16_t speed; /* 28 m/s * 16 */ - int16_t height; /* 30 m */ - /* 32 */ -}; - - -#define AO_TELEMETRY_METRUM_SENSOR 0x0A - -struct ao_telemetry_metrum_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t accel; /* 6 Z axis */ - - int32_t pres; /* 8 Pa * 10 */ - int16_t temp; /* 12 °C * 100 */ - - int16_t acceleration; /* 14 m/s² * 16 */ - int16_t speed; /* 16 m/s * 16 */ - int16_t height; /* 18 m */ - - int16_t v_batt; /* 20 battery voltage */ - int16_t sense_a; /* 22 apogee continuity sense */ - int16_t sense_m; /* 24 main continuity sense */ - - uint8_t pad[6]; /* 26 */ - /* 32 */ -}; - -#define AO_TELEMETRY_METRUM_DATA 0x0B - -struct ao_telemetry_metrum_data { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - int32_t ground_pres; /* 8 average pres on pad */ - int16_t ground_accel; /* 12 average accel on pad */ - int16_t accel_plus_g; /* 14 accel calibration at +1g */ - int16_t accel_minus_g; /* 16 accel calibration at -1g */ - - uint8_t pad[14]; /* 18 */ - /* 32 */ -}; - -#define AO_TELEMETRY_MINI 0x10 - -struct ao_telemetry_mini { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t v_batt; /* 6 battery voltage */ - int16_t sense_a; /* 8 apogee continuity */ - int16_t sense_m; /* 10 main continuity */ - - int32_t pres; /* 12 Pa * 10 */ - int16_t temp; /* 16 °C * 100 */ - - int16_t acceleration; /* 18 m/s² * 16 */ - int16_t speed; /* 20 m/s * 16 */ - int16_t height; /* 22 m */ - - int32_t ground_pres; /* 24 average pres on pad */ - - int32_t pad28; /* 28 */ - /* 32 */ -}; - -/* #define AO_SEND_ALL_BARO */ - -#define AO_TELEMETRY_BARO 0x80 - -/* - * This packet allows the full sampling rate baro - * data to be captured over the RF link so that the - * flight software can be tested using 'real' data. - * - * Along with this telemetry packet, the flight - * code is modified to send full-rate telemetry all the time - * and never send an RDF tone; this ensure that the full radio - * link is available. - */ -struct ao_telemetry_baro { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t samples; /* 5 number samples */ - - int16_t baro[12]; /* 6 samples */ - /* 32 */ -}; - -union ao_telemetry_all { - struct ao_telemetry_generic generic; - struct ao_telemetry_sensor sensor; - struct ao_telemetry_configuration configuration; - struct ao_telemetry_location location; - struct ao_telemetry_satellite satellite; - struct ao_telemetry_companion companion; - struct ao_telemetry_mega_sensor mega_sensor; - struct ao_telemetry_mega_data mega_data; - struct ao_telemetry_metrum_sensor metrum_sensor; - struct ao_telemetry_metrum_data metrum_data; - struct ao_telemetry_mini mini; - struct ao_telemetry_baro baro; -}; - -struct ao_telemetry_all_recv { - union ao_telemetry_all telemetry; - int8_t rssi; - uint8_t status; -}; - -#endif /* _AO_TELEMETRY_H_ */ diff --git a/src/core/ao_usb.h b/src/core/ao_usb.h deleted file mode 100644 index 1ce4f82f..00000000 --- a/src/core/ao_usb.h +++ /dev/null @@ -1,142 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. - */ - -#ifndef _AO_USB_H_ -#define _AO_USB_H_ - -/* - * ao_usb.c - */ - -/* Put one character to the USB output queue */ -void -ao_usb_putchar(char c); - -/* Get one character from the USB input queue */ -char -ao_usb_getchar(void); - -/* Poll for a charcter on the USB input queue. - * returns AO_READ_AGAIN if none are available - */ -int -ao_usb_pollchar(void); - -/* Flush the USB output queue */ -void -ao_usb_flush(void); - -/* Enable the USB controller */ -void -ao_usb_enable(void); - -/* Disable the USB controller */ -void -ao_usb_disable(void); - -/* Initialize the USB system */ -void -ao_usb_init(void); - -extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; - -#define AO_USB_SETUP_DIR_MASK (0x01 << 7) -#define AO_USB_SETUP_TYPE_MASK (0x03 << 5) -#define AO_USB_SETUP_RECIP_MASK (0x1f) - -#define AO_USB_DIR_OUT 0 -#define AO_USB_DIR_IN (1 << 7) - -#define AO_USB_TYPE_STANDARD 0 -#define AO_USB_TYPE_CLASS (1 << 5) -#define AO_USB_TYPE_VENDOR (2 << 5) -#define AO_USB_TYPE_RESERVED (3 << 5) - -#define AO_USB_RECIP_DEVICE 0 -#define AO_USB_RECIP_INTERFACE 1 -#define AO_USB_RECIP_ENDPOINT 2 -#define AO_USB_RECIP_OTHER 3 - -/* standard requests */ -#define AO_USB_REQ_GET_STATUS 0x00 -#define AO_USB_REQ_CLEAR_FEATURE 0x01 -#define AO_USB_REQ_SET_FEATURE 0x03 -#define AO_USB_REQ_SET_ADDRESS 0x05 -#define AO_USB_REQ_GET_DESCRIPTOR 0x06 -#define AO_USB_REQ_SET_DESCRIPTOR 0x07 -#define AO_USB_REQ_GET_CONFIGURATION 0x08 -#define AO_USB_REQ_SET_CONFIGURATION 0x09 -#define AO_USB_REQ_GET_INTERFACE 0x0A -#define AO_USB_REQ_SET_INTERFACE 0x0B -#define AO_USB_REQ_SYNCH_FRAME 0x0C - -#define AO_USB_DESC_DEVICE 1 -#define AO_USB_DESC_CONFIGURATION 2 -#define AO_USB_DESC_STRING 3 -#define AO_USB_DESC_INTERFACE 4 -#define AO_USB_DESC_ENDPOINT 5 -#define AO_USB_DESC_DEVICE_QUALIFIER 6 -#define AO_USB_DESC_OTHER_SPEED 7 -#define AO_USB_DESC_INTERFACE_POWER 8 - -#define AO_USB_GET_DESC_TYPE(x) (((x)>>8)&0xFF) -#define AO_USB_GET_DESC_INDEX(x) ((x)&0xFF) - -#define AO_USB_CONTROL_EP 0 -#define AO_USB_CONTROL_SIZE 32 - -#define AO_USB_INT_EP 1 -#define AO_USB_INT_SIZE 8 - -#ifndef AO_USB_OUT_EP -#define AO_USB_OUT_EP 4 -#define AO_USB_IN_EP 5 -#endif - -/* - * USB bulk packets can only come in 8, 16, 32 and 64 - * byte sizes, so we'll use 64 for everything - */ -#define AO_USB_IN_SIZE 64 -#define AO_USB_OUT_SIZE 64 - -#define AO_USB_EP0_IDLE 0 -#define AO_USB_EP0_DATA_IN 1 -#define AO_USB_EP0_DATA_OUT 2 -#define AO_USB_EP0_STALL 3 - -#define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) - -/* CDC definitions */ -#define AO_USB_CS_INTERFACE 0x24 -#define AO_USB_CS_ENDPOINT 0x25 - -#define AO_USB_SET_LINE_CODING 0x20 -#define AO_USB_GET_LINE_CODING 0x21 -#define AO_USB_SET_CONTROL_LINE_STATE 0x22 - -/* Data structure for GET_LINE_CODING / SET_LINE_CODING class requests */ -struct ao_usb_line_coding { - uint32_t rate; - uint8_t char_format; - uint8_t parity; - uint8_t data_bits; -} ; - -extern __pdata uint8_t ao_usb_running; - -#endif /* _AO_USB_H_ */ diff --git a/src/kernel/altitude.h b/src/kernel/altitude.h new file mode 100644 index 00000000..a278bbc6 --- /dev/null +++ b/src/kernel/altitude.h @@ -0,0 +1,132 @@ +/*max error 3.197865153490684 at 0.782%. Average error 0.260150920474668*/ +#define NALT 129 +#define ALT_FRAC_BITS 8 + 15835, /* 10.56 kPa 0.000% */ + 15332, /* 11.42 kPa 0.781% */ + 14868, /* 12.29 kPa 1.563% */ + 14435, /* 13.16 kPa 2.344% */ + 14030, /* 14.02 kPa 3.125% */ + 13649, /* 14.90 kPa 3.906% */ + 13290, /* 15.76 kPa 4.688% */ + 12950, /* 16.63 kPa 5.469% */ + 12627, /* 17.50 kPa 6.250% */ + 12320, /* 18.37 kPa 7.031% */ + 12027, /* 19.24 kPa 7.813% */ + 11747, /* 20.10 kPa 8.594% */ + 11479, /* 20.97 kPa 9.375% */ + 11222, /* 21.84 kPa 10.156% */ + 10975, /* 22.71 kPa 10.938% */ + 10736, /* 23.58 kPa 11.719% */ + 10504, /* 24.44 kPa 12.500% */ + 10278, /* 25.31 kPa 13.281% */ + 10059, /* 26.18 kPa 14.063% */ + 9846, /* 27.05 kPa 14.844% */ + 9638, /* 27.91 kPa 15.625% */ + 9435, /* 28.78 kPa 16.406% */ + 9237, /* 29.65 kPa 17.188% */ + 9044, /* 30.52 kPa 17.969% */ + 8855, /* 31.39 kPa 18.750% */ + 8670, /* 32.26 kPa 19.531% */ + 8490, /* 33.13 kPa 20.313% */ + 8313, /* 33.99 kPa 21.094% */ + 8140, /* 34.86 kPa 21.875% */ + 7970, /* 35.73 kPa 22.656% */ + 7803, /* 36.60 kPa 23.438% */ + 7640, /* 37.47 kPa 24.219% */ + 7480, /* 38.33 kPa 25.000% */ + 7322, /* 39.20 kPa 25.781% */ + 7168, /* 40.07 kPa 26.563% */ + 7016, /* 40.94 kPa 27.344% */ + 6867, /* 41.80 kPa 28.125% */ + 6720, /* 42.67 kPa 28.906% */ + 6575, /* 43.54 kPa 29.688% */ + 6433, /* 44.41 kPa 30.469% */ + 6294, /* 45.28 kPa 31.250% */ + 6156, /* 46.15 kPa 32.031% */ + 6020, /* 47.01 kPa 32.813% */ + 5887, /* 47.88 kPa 33.594% */ + 5755, /* 48.75 kPa 34.375% */ + 5625, /* 49.62 kPa 35.156% */ + 5497, /* 50.49 kPa 35.938% */ + 5371, /* 51.35 kPa 36.719% */ + 5247, /* 52.22 kPa 37.500% */ + 5124, /* 53.09 kPa 38.281% */ + 5003, /* 53.96 kPa 39.063% */ + 4883, /* 54.83 kPa 39.844% */ + 4765, /* 55.69 kPa 40.625% */ + 4648, /* 56.56 kPa 41.406% */ + 4533, /* 57.43 kPa 42.188% */ + 4419, /* 58.30 kPa 42.969% */ + 4307, /* 59.17 kPa 43.750% */ + 4196, /* 60.03 kPa 44.531% */ + 4086, /* 60.90 kPa 45.313% */ + 3977, /* 61.77 kPa 46.094% */ + 3870, /* 62.63 kPa 46.875% */ + 3764, /* 63.51 kPa 47.656% */ + 3659, /* 64.38 kPa 48.438% */ + 3555, /* 65.24 kPa 49.219% */ + 3453, /* 66.11 kPa 50.000% */ + 3351, /* 66.98 kPa 50.781% */ + 3250, /* 67.85 kPa 51.563% */ + 3151, /* 68.72 kPa 52.344% */ + 3052, /* 69.58 kPa 53.125% */ + 2955, /* 70.45 kPa 53.906% */ + 2858, /* 71.32 kPa 54.688% */ + 2763, /* 72.19 kPa 55.469% */ + 2668, /* 73.06 kPa 56.250% */ + 2574, /* 73.92 kPa 57.031% */ + 2482, /* 74.79 kPa 57.813% */ + 2390, /* 75.66 kPa 58.594% */ + 2298, /* 76.52 kPa 59.375% */ + 2208, /* 77.40 kPa 60.156% */ + 2119, /* 78.26 kPa 60.938% */ + 2030, /* 79.13 kPa 61.719% */ + 1942, /* 80.00 kPa 62.500% */ + 1855, /* 80.87 kPa 63.281% */ + 1769, /* 81.74 kPa 64.063% */ + 1683, /* 82.60 kPa 64.844% */ + 1598, /* 83.47 kPa 65.625% */ + 1514, /* 84.34 kPa 66.406% */ + 1430, /* 85.21 kPa 67.188% */ + 1347, /* 86.08 kPa 67.969% */ + 1265, /* 86.94 kPa 68.750% */ + 1184, /* 87.81 kPa 69.531% */ + 1103, /* 88.68 kPa 70.313% */ + 1023, /* 89.55 kPa 71.094% */ + 943, /* 90.41 kPa 71.875% */ + 864, /* 91.28 kPa 72.656% */ + 786, /* 92.15 kPa 73.438% */ + 708, /* 93.02 kPa 74.219% */ + 631, /* 93.89 kPa 75.000% */ + 554, /* 94.76 kPa 75.781% */ + 478, /* 95.63 kPa 76.563% */ + 403, /* 96.49 kPa 77.344% */ + 328, /* 97.36 kPa 78.125% */ + 254, /* 98.23 kPa 78.906% */ + 180, /* 99.10 kPa 79.688% */ + 106, /* 99.97 kPa 80.469% */ + 34, /* 100.83 kPa 81.250% */ + -39, /* 101.70 kPa 82.031% */ + -111, /* 102.57 kPa 82.813% */ + -182, /* 103.44 kPa 83.594% */ + -253, /* 104.30 kPa 84.375% */ + -323, /* 105.17 kPa 85.156% */ + -393, /* 106.04 kPa 85.938% */ + -462, /* 106.91 kPa 86.719% */ + -531, /* 107.78 kPa 87.500% */ + -600, /* 108.65 kPa 88.281% */ + -668, /* 109.51 kPa 89.063% */ + -736, /* 110.38 kPa 89.844% */ + -803, /* 111.25 kPa 90.625% */ + -870, /* 112.12 kPa 91.406% */ + -936, /* 112.99 kPa 92.188% */ + -1002, /* 113.85 kPa 92.969% */ + -1068, /* 114.72 kPa 93.750% */ + -1133, /* 115.59 kPa 94.531% */ + -1198, /* 116.46 kPa 95.313% */ + -1262, /* 117.33 kPa 96.094% */ + -1326, /* 118.19 kPa 96.875% */ + -1389, /* 119.06 kPa 97.656% */ + -1453, /* 119.93 kPa 98.438% */ + -1516, /* 120.80 kPa 99.219% */ + -1578, /* 121.67 kPa 100.000% */ diff --git a/src/kernel/ao.h b/src/kernel/ao.h new file mode 100644 index 00000000..29ad2603 --- /dev/null +++ b/src/kernel/ao.h @@ -0,0 +1,1041 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_H_ +#define _AO_H_ + +#include +#include +#include +#include +#include +#include + +#define TRUE 1 +#define FALSE 0 + +/* Convert a __data pointer into an __xdata pointer */ +#ifndef DATA_TO_XDATA +#define DATA_TO_XDATA(a) (a) +#endif +#ifndef PDATA_TO_XDATA +#define PDATA_TO_XDATA(a) (a) +#endif +#ifndef CODE_TO_XDATA +#define CODE_TO_XDATA(a) (a) +#endif + +#ifndef HAS_TASK +#define HAS_TASK 1 +#endif + +#ifndef AO_PORT_TYPE +#define AO_PORT_TYPE uint8_t +#endif + +typedef AO_PORT_TYPE ao_port_t; + +#if HAS_TASK +#include +#else +#include +#endif + +/* + * ao_panic.c + */ + +#define AO_PANIC_NO_TASK 1 /* AO_NUM_TASKS is not large enough */ +#define AO_PANIC_DMA 2 /* Attempt to start DMA while active */ +#define AO_PANIC_MUTEX 3 /* Mis-using mutex API */ +#define AO_PANIC_EE 4 /* Mis-using eeprom API */ +#define AO_PANIC_LOG 5 /* Failing to read/write log data */ +#define AO_PANIC_CMD 6 /* Too many command sets registered */ +#define AO_PANIC_STDIO 7 /* Too many stdio handlers registered */ +#define AO_PANIC_REBOOT 8 /* Reboot failed */ +#define AO_PANIC_FLASH 9 /* Invalid flash part (or wrong blocksize) */ +#define AO_PANIC_USB 10 /* Trying to send USB packet while busy */ +#define AO_PANIC_BT 11 /* Communications with bluetooth device failed */ +#define AO_PANIC_STACK 12 /* Stack overflow */ +#define AO_PANIC_SPI 13 /* SPI communication failure */ +#define AO_PANIC_CRASH 14 /* Processor crashed */ +#define AO_PANIC_BUFIO 15 /* Mis-using bufio API */ +#define AO_PANIC_EXTI 16 /* Mis-using exti API */ +#define AO_PANIC_FAST_TIMER 17 /* Mis-using fast timer API */ +#define AO_PANIC_SELF_TEST_CC1120 0x40 | 1 /* Self test failure */ +#define AO_PANIC_SELF_TEST_HMC5883 0x40 | 2 /* Self test failure */ +#define AO_PANIC_SELF_TEST_MPU6000 0x40 | 3 /* Self test failure */ +#define AO_PANIC_SELF_TEST_MS5607 0x40 | 4 /* Self test failure */ + +/* Stop the operating system, beeping and blinking the reason */ +void +ao_panic(uint8_t reason); + +/* + * ao_timer.c + */ + +#ifndef AO_TICK_TYPE +#define AO_TICK_TYPE uint16_t +#define AO_TICK_SIGNED int16_t +#endif + +extern volatile __data AO_TICK_TYPE ao_tick_count; + +/* Our timer runs at 100Hz */ +#ifndef AO_HERTZ +#define AO_HERTZ 100 +#endif +#define AO_MS_TO_TICKS(ms) ((ms) / (1000 / AO_HERTZ)) +#define AO_SEC_TO_TICKS(s) ((s) * AO_HERTZ) + +/* Returns the current time in ticks */ +AO_TICK_TYPE +ao_time(void); + +/* Suspend the current task until ticks time has passed */ +void +ao_delay(uint16_t ticks); + +/* Set the ADC interval */ +void +ao_timer_set_adc_interval(uint8_t interval); + +/* Timer interrupt */ +void +ao_timer_isr(void) ao_arch_interrupt(9); + +/* Initialize the timer */ +void +ao_timer_init(void); + +/* Initialize the hardware clock. Must be called first */ +void +ao_clock_init(void); + +/* + * ao_mutex.c + */ + +#ifndef ao_mutex_get +void +ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant; + +void +ao_mutex_put(__xdata uint8_t *ao_mutex) __reentrant; +#endif + +/* + * ao_cmd.c + */ + +enum ao_cmd_status { + ao_cmd_success = 0, + ao_cmd_lex_error = 1, + ao_cmd_syntax_error = 2, +}; + +extern __pdata uint16_t ao_cmd_lex_i; +extern __pdata uint32_t ao_cmd_lex_u32; +extern __pdata char ao_cmd_lex_c; +extern __pdata enum ao_cmd_status ao_cmd_status; + +void +ao_put_string(__code char *s); + +void +ao_cmd_lex(void); + +void +ao_cmd_put8(uint8_t v); + +void +ao_cmd_put16(uint16_t v); + +uint8_t +ao_cmd_is_white(void); + +void +ao_cmd_white(void); + +int8_t +ao_cmd_hexchar(char c); + +void +ao_cmd_hexbyte(void); + +void +ao_cmd_hex(void); + +void +ao_cmd_decimal(void) __reentrant; + +/* Read a single hex nibble off stdin. */ +uint8_t +ao_getnibble(void); + +uint8_t +ao_match_word(__code char *word); + +struct ao_cmds { + void (*func)(void); + __code char *help; +}; + +void +ao_cmd_register(const __code struct ao_cmds *cmds); + +void +ao_cmd_init(void); + +#if HAS_CMD_FILTER +/* + * Provided by an external module to filter raw command lines + */ +uint8_t +ao_cmd_filter(void); +#endif + +/* + * Various drivers + */ +#if HAS_ADC +#include +#endif + +#if HAS_BEEP +#include +#endif + +#if LEDS_AVAILABLE +#include +#endif + +#if HAS_USB +#include +#endif + +#if HAS_EEPROM +#include +#endif + +#if HAS_LOG +#include +#endif + +#if HAS_FLIGHT +#include +#include +#endif + +/* + * ao_report.c + */ + +#define AO_RDF_INTERVAL_TICKS AO_SEC_TO_TICKS(5) +#define AO_RDF_LENGTH_MS 500 +#define AO_RDF_CONTINUITY_MS 32 +#define AO_RDF_CONTINUITY_PAUSE 96 +#define AO_RDF_CONTINUITY_TOTAL ((AO_RDF_CONTINUITY_PAUSE + AO_RDF_CONTINUITY_MS) * 3 + AO_RDF_CONTINUITY_PAUSE) + +/* This assumes that we're generating a 1kHz tone, which + * modulates the carrier at 2kbps, or 250kBps + */ +#define AO_MS_TO_RDF_LEN(ms) ((ms) / 4) + +#define AO_RADIO_RDF_LEN AO_MS_TO_RDF_LEN(AO_RDF_LENGTH_MS) +#define AO_RADIO_CONT_TONE_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_MS) +#define AO_RADIO_CONT_PAUSE_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_PAUSE) +#define AO_RADIO_CONT_TOTAL_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_TOTAL) + +/* returns a value 0-3 to indicate igniter continuity */ +uint8_t +ao_report_igniter(void); + +void +ao_report_init(void); + +/* + * ao_convert.c + * + * Given raw data, convert to SI units + */ + +/* pressure from the sensor to altitude in meters */ +int16_t +ao_pres_to_altitude(int16_t pres) __reentrant; + +int16_t +ao_altitude_to_pres(int16_t alt) __reentrant; + +int16_t +ao_temp_to_dC(int16_t temp) __reentrant; + +/* + * ao_convert_pa.c + * + * Convert between pressure in Pa and altitude in meters + */ + +#include + +alt_t +ao_pa_to_altitude(int32_t pa); + +int32_t +ao_altitude_to_pa(alt_t alt); + +#if HAS_DBG +#include +#endif + +#if HAS_SERIAL_0 || HAS_SERIAL_1 || HAS_SERIAL_2 || HAS_SERIAL_3 +#include +#endif + +/* + * ao_convert_volt.c + * + * Convert ADC readings to decivolts + */ + +int16_t +ao_battery_decivolt(int16_t adc); + +int16_t +ao_ignite_decivolt(int16_t adc); + +/* + * ao_spi_slave.c + */ + +uint8_t +ao_spi_slave_recv(void *buf, uint16_t len); + +void +ao_spi_slave_send(void *buf, uint16_t len); + +void +ao_spi_slave_init(void); + +/* This must be defined by the product; it will get called when chip + * select goes low, at which point it should use ao_spi_read and + * ao_spi_write to deal with the request + */ + +void +ao_spi_slave(void); + +#include +/* + * ao_gps.c + */ + +#define AO_GPS_NUM_SAT_MASK (0xf << 0) +#define AO_GPS_NUM_SAT_SHIFT (0) + +#define AO_GPS_VALID (1 << 4) +#define AO_GPS_RUNNING (1 << 5) +#define AO_GPS_DATE_VALID (1 << 6) +#define AO_GPS_COURSE_VALID (1 << 7) + +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +extern __xdata uint8_t ao_gps_new; +extern __pdata uint16_t ao_gps_tick; +extern __xdata uint8_t ao_gps_mutex; +extern __xdata struct ao_telemetry_location ao_gps_data; +extern __xdata struct ao_telemetry_satellite ao_gps_tracking_data; + +struct ao_gps_orig { + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t flags; + int32_t latitude; /* degrees * 10⁷ */ + int32_t longitude; /* degrees * 10⁷ */ + int16_t altitude; /* m */ + uint16_t ground_speed; /* cm/s */ + uint8_t course; /* degrees / 2 */ + uint8_t hdop; /* * 5 */ + int16_t climb_rate; /* cm/s */ + uint16_t h_error; /* m */ + uint16_t v_error; /* m */ +}; + +struct ao_gps_sat_orig { + uint8_t svid; + uint8_t c_n_1; +}; + +#define AO_MAX_GPS_TRACKING 12 + +struct ao_gps_tracking_orig { + uint8_t channels; + struct ao_gps_sat_orig sats[AO_MAX_GPS_TRACKING]; +}; + +void +ao_gps(void); + +void +ao_gps_print(__xdata struct ao_gps_orig *gps_data); + +void +ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data); + +void +ao_gps_show(void) __reentrant; + +void +ao_gps_init(void); + +/* + * ao_gps_report.c + */ + +void +ao_gps_report(void); + +void +ao_gps_report_init(void); + +/* + * ao_gps_report_mega.c + */ + +void +ao_gps_report_mega(void); + +void +ao_gps_report_mega_init(void); + +/* + * ao_telemetry_orig.c + */ + +#if LEGACY_MONITOR +struct ao_adc_orig { + uint16_t tick; /* tick when the sample was read */ + int16_t accel; /* accelerometer */ + int16_t pres; /* pressure sensor */ + int16_t temp; /* temperature sensor */ + int16_t v_batt; /* battery voltage */ + int16_t sense_d; /* drogue continuity sense */ + int16_t sense_m; /* main continuity sense */ +}; + +struct ao_telemetry_orig { + uint16_t serial; + uint16_t flight; + uint8_t flight_state; + int16_t accel; + int16_t ground_accel; + union { + struct { + int16_t speed; + int16_t unused; + } k; + int32_t flight_vel; + } u; + int16_t height; + int16_t ground_pres; + int16_t accel_plus_g; + int16_t accel_minus_g; + struct ao_adc_orig adc; + struct ao_gps_orig gps; + char callsign[AO_MAX_CALLSIGN]; + struct ao_gps_tracking_orig gps_tracking; +}; + +struct ao_telemetry_tiny { + uint16_t serial; + uint16_t flight; + uint8_t flight_state; + int16_t height; /* AGL in meters */ + int16_t speed; /* in m/s * 16 */ + int16_t accel; /* in m/s² * 16 */ + int16_t ground_pres; /* sensor units */ + struct ao_adc adc; /* raw ADC readings */ + char callsign[AO_MAX_CALLSIGN]; +}; + +struct ao_telemetry_orig_recv { + struct ao_telemetry_orig telemetry_orig; + int8_t rssi; + uint8_t status; +}; + +struct ao_telemetry_tiny_recv { + struct ao_telemetry_tiny telemetry_tiny; + int8_t rssi; + uint8_t status; +}; + +#endif /* LEGACY_MONITOR */ + +/* Unfortunately, we've exposed the CC1111 rssi units as the 'usual' method + * for reporting RSSI. So, now we use these values everywhere + */ +#define AO_RSSI_FROM_RADIO(radio) ((int16_t) ((int8_t) (radio) >> 1) - 74) +#define AO_RADIO_FROM_RSSI(rssi) (((int8_t) (rssi) + 74) << 1) + +/* + * ao_radio_recv tacks on rssi and status bytes + */ + +struct ao_telemetry_raw_recv { + uint8_t packet[AO_MAX_TELEMETRY + 2]; +}; + +/* Set delay between telemetry reports (0 to disable) */ + +#ifdef AO_SEND_ALL_BARO +#define AO_TELEMETRY_INTERVAL_PAD AO_MS_TO_TICKS(100) +#define AO_TELEMETRY_INTERVAL_FLIGHT AO_MS_TO_TICKS(100) +#define AO_TELEMETRY_INTERVAL_RECOVER AO_MS_TO_TICKS(100) +#else +#define AO_TELEMETRY_INTERVAL_PAD AO_MS_TO_TICKS(1000) +#define AO_TELEMETRY_INTERVAL_FLIGHT AO_MS_TO_TICKS(100) +#define AO_TELEMETRY_INTERVAL_RECOVER AO_MS_TO_TICKS(1000) +#endif + +void +ao_telemetry_set_interval(uint16_t interval); + +void +ao_rdf_set(uint8_t rdf); + +void +ao_telemetry_init(void); + +void +ao_telemetry_orig_init(void); + +void +ao_telemetry_tiny_init(void); + +/* + * ao_radio.c + */ + +extern __xdata uint8_t ao_radio_dma; + +extern __xdata int8_t ao_radio_rssi; + +#ifdef PKT_APPEND_STATUS_1_CRC_OK +#define AO_RADIO_STATUS_CRC_OK PKT_APPEND_STATUS_1_CRC_OK +#else +#include +#define AO_RADIO_STATUS_CRC_OK AO_FEC_DECODE_CRC_OK +#endif + +#ifndef HAS_RADIO_RECV +#define HAS_RADIO_RECV HAS_RADIO +#endif +#ifndef HAS_RADIO_XMIT +#define HAS_RADIO_XMIT HAS_RADIO +#endif + +void +ao_radio_general_isr(void) ao_arch_interrupt(16); + +#if HAS_RADIO_XMIT +void +ao_radio_send(const __xdata void *d, uint8_t size) __reentrant; +#endif + +#if HAS_RADIO_RECV +uint8_t +ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout) __reentrant; + +void +ao_radio_recv_abort(void); +#endif + +void +ao_radio_test(uint8_t on); + +typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len); + +void +ao_radio_send_aprs(ao_radio_fill_func fill); + +/* + * ao_radio_pa + */ + +#if HAS_RADIO_AMP +void +ao_radio_pa_on(void); + +void +ao_radio_pa_off(void); + +void +ao_radio_pa_init(void); +#else +#define ao_radio_pa_on() +#define ao_radio_pa_off() +#define ao_radio_pa_init() +#endif + +/* + * Compute the packet length as follows: + * + * 2000 bps (for a 1kHz tone) + * so, for 'ms' milliseconds, we need + * 2 * ms bits, or ms / 4 bytes + */ + +void +ao_radio_rdf(void); + +void +ao_radio_continuity(uint8_t c); + +void +ao_radio_rdf_abort(void); + +void +ao_radio_init(void); + +/* + * ao_monitor.c + */ + +#if HAS_MONITOR + +extern const char const * const ao_state_names[]; + +#define AO_MONITOR_RING 8 + +union ao_monitor { + struct ao_telemetry_raw_recv raw; + struct ao_telemetry_all_recv all; +#if LEGACY_MONITOR + struct ao_telemetry_orig_recv orig; + struct ao_telemetry_tiny_recv tiny; +#endif +}; + +extern __xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; + +#define ao_monitor_ring_next(n) (((n) + 1) & (AO_MONITOR_RING - 1)) + +extern __data uint8_t ao_monitoring; +extern __data uint8_t ao_monitor_head; + +void +ao_monitor(void); + +#define AO_MONITORING_OFF 0 +#define AO_MONITORING_ORIG 1 + +void +ao_monitor_set(uint8_t monitoring); + +void +ao_monitor_disable(void); + +void +ao_monitor_enable(void); + +void +ao_monitor_init(void) __reentrant; + +#endif + +/* + * ao_stdio.c + */ + +#define AO_READ_AGAIN (-1) + +struct ao_stdio { + int (*_pollchar)(void); /* Called with interrupts blocked */ + void (*putchar)(char c) __reentrant; + void (*flush)(void); + uint8_t echo; +}; + +extern __xdata struct ao_stdio ao_stdios[]; +extern __pdata int8_t ao_cur_stdio; +extern __pdata int8_t ao_num_stdios; + +void +flush(void); + +extern __xdata uint8_t ao_stdin_ready; + +uint8_t +ao_echo(void); + +int8_t +ao_add_stdio(int (*pollchar)(void), + void (*putchar)(char) __reentrant, + void (*flush)(void)) __reentrant; + +/* + * ao_ignite.c + */ + +enum ao_igniter { + ao_igniter_drogue = 0, + ao_igniter_main = 1 +}; + +void +ao_ignite(enum ao_igniter igniter); + +enum ao_igniter_status { + ao_igniter_unknown, /* unknown status (ambiguous voltage) */ + ao_igniter_ready, /* continuity detected */ + ao_igniter_active, /* igniter firing */ + ao_igniter_open, /* open circuit detected */ +}; + +struct ao_ignition { + uint8_t request; + uint8_t fired; + uint8_t firing; +}; + +extern __code char * __code ao_igniter_status_names[]; + +extern __xdata struct ao_ignition ao_ignition[2]; + +enum ao_igniter_status +ao_igniter_status(enum ao_igniter igniter); + +extern __pdata uint8_t ao_igniter_present; + +void +ao_ignite_set_pins(void); + +void +ao_igniter_init(void); + +/* + * ao_config.c + */ + +#if AO_PYRO_NUM +#include +#endif + +#if HAS_FORCE_FREQ +/* + * Set this to force the frequency to 434.550MHz + */ +extern __xdata uint8_t ao_force_freq; +#endif + +#define AO_CONFIG_MAJOR 1 +#define AO_CONFIG_MINOR 15 + +#define AO_AES_LEN 16 + +extern __xdata uint8_t ao_config_aes_seq; + +struct ao_config { + uint8_t major; + uint8_t minor; + uint16_t main_deploy; + int16_t accel_plus_g; /* changed for minor version 2 */ + uint8_t _legacy_radio_channel; + char callsign[AO_MAX_CALLSIGN + 1]; + uint8_t apogee_delay; /* minor version 1 */ + int16_t accel_minus_g; /* minor version 2 */ + uint32_t radio_cal; /* minor version 3 */ + uint32_t flight_log_max; /* minor version 4 */ + uint8_t ignite_mode; /* minor version 5 */ + uint8_t pad_orientation; /* minor version 6 */ + uint32_t radio_setting; /* minor version 7 */ + uint8_t radio_enable; /* minor version 8 */ + uint8_t aes_key[AO_AES_LEN]; /* minor version 9 */ + uint32_t frequency; /* minor version 10 */ + uint16_t apogee_lockout; /* minor version 11 */ +#if AO_PYRO_NUM + struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ +#endif + uint16_t aprs_interval; /* minor version 13 */ +#if HAS_RADIO_POWER + uint8_t radio_power; /* minor version 14 */ +#endif +#if HAS_RADIO_AMP + uint8_t radio_amp; /* minor version 14 */ +#endif +#if HAS_GYRO + int16_t accel_zero_along; /* minor version 15 */ + int16_t accel_zero_across; /* minor version 15 */ + int16_t accel_zero_through; /* minor version 15 */ +#endif +}; + +#define AO_IGNITE_MODE_DUAL 0 +#define AO_IGNITE_MODE_APOGEE 1 +#define AO_IGNITE_MODE_MAIN 2 + +#define AO_RADIO_ENABLE_CORE 1 +#define AO_RADIO_DISABLE_TELEMETRY 2 +#define AO_RADIO_DISABLE_RDF 4 + +#define AO_PAD_ORIENTATION_ANTENNA_UP 0 +#define AO_PAD_ORIENTATION_ANTENNA_DOWN 1 + +extern __xdata struct ao_config ao_config; + +#define AO_CONFIG_MAX_SIZE 128 + +void +_ao_config_edit_start(void); + +void +_ao_config_edit_finish(void); + +void +ao_config_get(void); + +void +ao_config_put(void); + +void +ao_config_set_radio(void); + +void +ao_config_init(void); + +/* + * ao_rssi.c + */ + +void +ao_rssi_set(int rssi_value); + +void +ao_rssi_init(uint8_t rssi_led); + +/* + * ao_product.c + * + * values which need to be defined for + * each instance of a product + */ + +extern const char ao_version[]; +extern const char ao_manufacturer[]; +extern const char ao_product[]; + +/* + * Fifos + */ + +#define AO_FIFO_SIZE 32 + +struct ao_fifo { + uint8_t insert; + uint8_t remove; + char fifo[AO_FIFO_SIZE]; +}; + +#define ao_fifo_insert(f,c) do { \ + (f).fifo[(f).insert] = (c); \ + (f).insert = ((f).insert + 1) & (AO_FIFO_SIZE-1); \ +} while(0) + +#define ao_fifo_remove(f,c) do {\ + c = (f).fifo[(f).remove]; \ + (f).remove = ((f).remove + 1) & (AO_FIFO_SIZE-1); \ +} while(0) + +#define ao_fifo_full(f) ((((f).insert + 1) & (AO_FIFO_SIZE-1)) == (f).remove) +#define ao_fifo_empty(f) ((f).insert == (f).remove) + +#if PACKET_HAS_MASTER || PACKET_HAS_SLAVE +#include +#endif + +#if HAS_BTM +#include +#endif + +#if HAS_COMPANION +#include +#endif + +#if HAS_LCD +#include +#endif + +#if HAS_AES +#include +#endif + +/* ao_launch.c */ + +struct ao_launch_command { + uint16_t tick; + uint16_t serial; + uint8_t cmd; + uint8_t channel; + uint16_t unused; +}; + +#define AO_LAUNCH_QUERY 1 + +struct ao_launch_query { + uint16_t tick; + uint16_t serial; + uint8_t channel; + uint8_t valid; + uint8_t arm_status; + uint8_t igniter_status; +}; + +#define AO_LAUNCH_ARM 2 +#define AO_LAUNCH_FIRE 3 + +void +ao_launch_init(void); + +/* + * ao_log_single.c + */ + +#define AO_LOG_TELESCIENCE_START ((uint8_t) 's') +#define AO_LOG_TELESCIENCE_DATA ((uint8_t) 'd') + +#define AO_LOG_TELESCIENCE_NUM_ADC 12 + +struct ao_log_telescience { + uint8_t type; + uint8_t csum; + uint16_t tick; + uint16_t tm_tick; + uint8_t tm_state; + uint8_t unused; + uint16_t adc[AO_LOG_TELESCIENCE_NUM_ADC]; +}; + +#define AO_LOG_SINGLE_SIZE 32 + +union ao_log_single { + struct ao_log_telescience telescience; + union ao_telemetry_all telemetry; + uint8_t bytes[AO_LOG_SINGLE_SIZE]; +}; + +extern __xdata union ao_log_single ao_log_single_write_data; +extern __xdata union ao_log_single ao_log_single_read_data; + +void +ao_log_single_extra_query(void); + +void +ao_log_single_list(void); + +void +ao_log_single_main(void); + +uint8_t +ao_log_single_write(void); + +uint8_t +ao_log_single_read(uint32_t pos); + +void +ao_log_single_start(void); + +void +ao_log_single_stop(void); + +void +ao_log_single_restart(void); + +void +ao_log_single_set(void); + +void +ao_log_single_delete(void); + +void +ao_log_single_init(void); + +void +ao_log_single(void); + +/* + * ao_pyro_slave.c + */ + +#define AO_TELEPYRO_NUM_ADC 9 + +#ifndef ao_xmemcpy +#define ao_xmemcpy(d,s,c) memcpy(d,s,c) +#define ao_xmemset(d,v,c) memset(d,v,c) +#define ao_xmemcmp(d,s,c) memcmp(d,s,c) +#endif + +/* + * ao_terraui.c + */ + +void +ao_terraui_init(void); + +/* + * ao_battery.c + */ + +#ifdef BATTERY_PIN +void +ao_battery_isr(void) ao_arch_interrupt(1); + +uint16_t +ao_battery_get(void); + +void +ao_battery_init(void); +#endif /* BATTERY_PIN */ + +/* + * ao_sqrt.c + */ + +uint32_t +ao_sqrt(uint32_t op); + +/* + * ao_freq.c + */ + +int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant; + +/* + * ao_ms5607.c + */ + +void ao_ms5607_init(void); + +#include + +#endif /* _AO_H_ */ diff --git a/src/kernel/ao_adc.h b/src/kernel/ao_adc.h new file mode 100644 index 00000000..373db1c4 --- /dev/null +++ b/src/kernel/ao_adc.h @@ -0,0 +1,35 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_ADC_H_ +#define _AO_ADC_H_ + +#include + +/* Trigger a conversion sequence (called from the timer interrupt) */ +void +ao_adc_poll(void); + +/* Suspend the current task until another A/D sample is converted */ +void +ao_adc_sleep(void); + +/* Initialize the A/D converter */ +void +ao_adc_init(void); + +#endif /* _AO_ADC_H_ */ diff --git a/src/kernel/ao_aes.h b/src/kernel/ao_aes.h new file mode 100644 index 00000000..c47bc2db --- /dev/null +++ b/src/kernel/ao_aes.h @@ -0,0 +1,54 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_AES_H_ +#define _AO_AES_H_ + +/* ao_aes.c */ + +extern __xdata uint8_t ao_aes_mutex; + +/* AES keys and blocks are 128 bits */ + +enum ao_aes_mode { + ao_aes_mode_cbc_mac +}; + +#if HAS_AES +#ifdef SDCC +void +ao_aes_isr(void) __interrupt 4; +#endif +#endif + +void +ao_aes_set_mode(enum ao_aes_mode mode); + +void +ao_aes_set_key(__xdata uint8_t *in); + +void +ao_aes_zero_iv(void); + +void +ao_aes_run(__xdata uint8_t *in, + __xdata uint8_t *out); + +void +ao_aes_init(void); + +#endif /* _AO_AES_H_ */ diff --git a/src/kernel/ao_balloon.c b/src/kernel/ao_balloon.c new file mode 100644 index 00000000..904a9c08 --- /dev/null +++ b/src/kernel/ao_balloon.c @@ -0,0 +1,136 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#endif + +#ifndef HAS_ACCEL +#error Please define HAS_ACCEL +#endif + +#ifndef HAS_GPS +#error Please define HAS_GPS +#endif + +#ifndef HAS_USB +#error Please define HAS_USB +#endif + +#if HAS_SENSOR_ERRORS +/* Any sensor can set this to mark the flight computer as 'broken' */ +__xdata uint8_t ao_sensor_errors; +#endif + +__pdata uint16_t ao_motor_number; /* number of motors burned so far */ + +/* Main flight thread. */ + +__pdata enum ao_flight_state ao_flight_state; /* current flight state */ + +__pdata uint8_t ao_flight_force_idle; + +void +ao_flight(void) +{ + ao_sample_init(); + ao_flight_state = ao_flight_startup; + for (;;) { + + /* + * Process ADC samples, just looping + * until the sensors are calibrated. + */ + if (!ao_sample()) + continue; + + switch (ao_flight_state) { + case ao_flight_startup: + + /* Check to see what mode we should go to. + * - Invalid mode if accel cal appears to be out + * - pad mode if we're upright, + * - idle mode otherwise + */ + if (!ao_flight_force_idle) + { + /* Set pad mode - we can fly! */ + ao_flight_state = ao_flight_pad; +#if HAS_USB + /* Disable the USB controller in flight mode + * to save power + */ + if (!ao_usb_running) + ao_usb_disable(); +#endif + + /* Disable packet mode in pad state */ + ao_packet_slave_stop(); + + /* Turn on telemetry system */ + ao_rdf_set(1); + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_BALLOON); + + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); + } else { + /* Set idle mode */ + ao_flight_state = ao_flight_idle; + + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); + } + /* wakeup threads due to state change */ + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + + break; + case ao_flight_pad: + + /* pad to coast: + * + * barometer: > 20m vertical motion + */ + if (ao_height > AO_M_TO_HEIGHT(20)) + { + ao_flight_state = ao_flight_drogue; + + /* start logging data */ + ao_log_start(); + +#if HAS_GPS + /* Record current GPS position by waking up GPS log tasks */ + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); +#endif + + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + break; + default: + break; + } + } +} + +static __xdata struct ao_task flight_task; + +void +ao_flight_init(void) +{ + ao_flight_state = ao_flight_startup; + ao_add_task(&flight_task, ao_flight, "flight"); +} diff --git a/src/kernel/ao_beep.h b/src/kernel/ao_beep.h new file mode 100644 index 00000000..55f61171 --- /dev/null +++ b/src/kernel/ao_beep.h @@ -0,0 +1,74 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_BEEP_H_ +#define _AO_BEEP_H_ + +/* + * ao_beep.c + */ + +/* + * Various pre-defined beep frequencies + * + * frequency = 1/2 (24e6/32) / beep + */ + +#define AO_BEEP_LOW 150 /* 2500Hz */ +#define AO_BEEP_MID 94 /* 3989Hz */ +#define AO_BEEP_HIGH 75 /* 5000Hz */ +#define AO_BEEP_OFF 0 /* off */ + +#define AO_BEEP_g 240 /* 1562.5Hz */ +#define AO_BEEP_gs 227 /* 1652Hz (1655Hz) */ +#define AO_BEEP_aa 214 /* 1752Hz (1754Hz) */ +#define AO_BEEP_bbf 202 /* 1856Hz (1858Hz) */ +#define AO_BEEP_bb 190 /* 1974Hz (1969Hz) */ +#define AO_BEEP_cc 180 /* 2083Hz (2086Hz) */ +#define AO_BEEP_ccs 170 /* 2205Hz (2210Hz) */ +#define AO_BEEP_dd 160 /* 2344Hz (2341Hz) */ +#define AO_BEEP_eef 151 /* 2483Hz (2480Hz) */ +#define AO_BEEP_ee 143 /* 2622Hz (2628Hz) */ +#define AO_BEEP_ff 135 /* 2778Hz (2784Hz) */ +#define AO_BEEP_ffs 127 /* 2953Hz (2950Hz) */ +#define AO_BEEP_gg 120 /* 3125Hz */ +#define AO_BEEP_ggs 113 /* 3319Hz (3311Hz) */ +#define AO_BEEP_aaa 107 /* 3504Hz (3508Hz) */ +#define AO_BEEP_bbbf 101 /* 3713Hz (3716Hz) */ +#define AO_BEEP_bbb 95 /* 3947Hz (3937Hz) */ +#define AO_BEEP_ccc 90 /* 4167Hz (4171Hz) */ +#define AO_BEEP_cccs 85 /* 4412Hz (4419Hz) */ +#define AO_BEEP_ddd 80 /* 4688Hz (4682Hz) */ +#define AO_BEEP_eeef 76 /* 4934Hz (4961Hz) */ +#define AO_BEEP_eee 71 /* 5282Hz (5256Hz) */ +#define AO_BEEP_fff 67 /* 5597Hz (5568Hz) */ +#define AO_BEEP_fffs 64 /* 5859Hz (5899Hz) */ +#define AO_BEEP_ggg 60 /* 6250Hz */ + +/* Set the beeper to the specified tone */ +void +ao_beep(uint8_t beep); + +/* Turn on the beeper for the specified time */ +void +ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant; + +/* Initialize the beeper */ +void +ao_beep_init(void); + +#endif /* _AO_BEEP_H_ */ diff --git a/src/kernel/ao_btm.h b/src/kernel/ao_btm.h new file mode 100644 index 00000000..484e5d7f --- /dev/null +++ b/src/kernel/ao_btm.h @@ -0,0 +1,36 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_BTM_H_ +#define _AO_BTM_H_ + +/* ao_btm.c */ + +/* If bt_link is on P2, this interrupt is shared by USB, so the USB + * code calls this function. Otherwise, it's a regular ISR. + */ + +void +ao_btm_isr(void) +#if BT_LINK_ON_P1 + __interrupt 15 +#endif + ; +void +ao_btm_init(void); + +#endif /* _AO_BTM_H_ */ diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c new file mode 100644 index 00000000..4ebaa607 --- /dev/null +++ b/src/kernel/ao_cmd.c @@ -0,0 +1,420 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_task.h" + +__pdata uint16_t ao_cmd_lex_i; +__pdata uint32_t ao_cmd_lex_u32; +__pdata char ao_cmd_lex_c; +__pdata enum ao_cmd_status ao_cmd_status; + +#define CMD_LEN 48 + +static __xdata char cmd_line[CMD_LEN]; +static __pdata uint8_t cmd_len; +static __pdata uint8_t cmd_i; + +void +ao_put_string(__code char *s) +{ + char c; + while ((c = *s++)) + putchar(c); +} + +static void +backspace(void) +{ + ao_put_string ("\010 \010"); +} + +static void +readline(void) +{ + char c; + if (ao_echo()) + ao_put_string("> "); + cmd_len = 0; + for (;;) { + flush(); + c = getchar(); + /* backspace/delete */ + if (c == '\010' || c == '\177') { + if (cmd_len != 0) { + if (ao_echo()) + backspace(); + --cmd_len; + } + continue; + } + + /* ^U */ + if (c == '\025') { + while (cmd_len != 0) { + if (ao_echo()) + backspace(); + --cmd_len; + } + continue; + } + + /* map CR to NL */ + if (c == '\r') + c = '\n'; + + if (c == '\n') { + if (ao_echo()) + putchar('\n'); + break; + } + + if (cmd_len >= CMD_LEN - 2) + continue; + cmd_line[cmd_len++] = c; + if (ao_echo()) + putchar(c); + } + cmd_line[cmd_len++] = '\n'; + cmd_line[cmd_len++] = '\0'; + cmd_i = 0; +} + +void +ao_cmd_lex(void) +{ + ao_cmd_lex_c = '\n'; + if (cmd_i < cmd_len) + ao_cmd_lex_c = cmd_line[cmd_i++]; +} + +static void +putnibble(uint8_t v) +{ + if (v < 10) + putchar(v + '0'); + else + putchar(v + ('a' - 10)); +} + +uint8_t +ao_getnibble(void) +{ + char c; + + c = getchar(); + if ('0' <= c && c <= '9') + return c - '0'; + if ('a' <= c && c <= 'f') + return c - ('a' - 10); + if ('A' <= c && c <= 'F') + return c - ('A' - 10); + ao_cmd_status = ao_cmd_lex_error; + return 0; +} + +void +ao_cmd_put16(uint16_t v) +{ + ao_cmd_put8(v >> 8); + ao_cmd_put8(v); +} + +void +ao_cmd_put8(uint8_t v) +{ + putnibble((v >> 4) & 0xf); + putnibble(v & 0xf); +} + +uint8_t +ao_cmd_is_white(void) +{ + return ao_cmd_lex_c == ' ' || ao_cmd_lex_c == '\t'; +} + +void +ao_cmd_white(void) +{ + while (ao_cmd_is_white()) + ao_cmd_lex(); +} + +int8_t +ao_cmd_hexchar(char c) +{ + if ('0' <= c && c <= '9') + return (c - '0'); + if ('a' <= c && c <= 'f') + return (c - 'a' + 10); + if ('A' <= c && c <= 'F') + return (c - 'A' + 10); + return -1; +} + +void +ao_cmd_hexbyte(void) +{ + uint8_t i; + int8_t n; + + ao_cmd_lex_i = 0; + ao_cmd_white(); + for (i = 0; i < 2; i++) { + n = ao_cmd_hexchar(ao_cmd_lex_c); + if (n < 0) { + ao_cmd_status = ao_cmd_syntax_error; + break; + } + ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n; + ao_cmd_lex(); + } +} + +void +ao_cmd_hex(void) +{ + __pdata uint8_t r = ao_cmd_lex_error; + int8_t n; + + ao_cmd_lex_i = 0; + ao_cmd_white(); + for(;;) { + n = ao_cmd_hexchar(ao_cmd_lex_c); + if (n < 0) + break; + ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n; + r = ao_cmd_success; + ao_cmd_lex(); + } + if (r != ao_cmd_success) + ao_cmd_status = r; +} + +void +ao_cmd_decimal(void) __reentrant +{ + uint8_t r = ao_cmd_lex_error; + + ao_cmd_lex_u32 = 0; + ao_cmd_white(); + for(;;) { + if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') + ao_cmd_lex_u32 = (ao_cmd_lex_u32 * 10) + (ao_cmd_lex_c - '0'); + else + break; + r = ao_cmd_success; + ao_cmd_lex(); + } + if (r != ao_cmd_success) + ao_cmd_status = r; + ao_cmd_lex_i = (uint16_t) ao_cmd_lex_u32; +} + +uint8_t +ao_match_word(__code char *word) +{ + while (*word) { + if (ao_cmd_lex_c != *word) { + ao_cmd_status = ao_cmd_syntax_error; + return 0; + } + word++; + ao_cmd_lex(); + } + return 1; +} + +static void +echo(void) +{ + ao_cmd_hex(); + if (ao_cmd_status == ao_cmd_success) + ao_stdios[ao_cur_stdio].echo = ao_cmd_lex_i != 0; +} + +static void +ao_reboot(void) +{ + ao_cmd_white(); + if (!ao_match_word("eboot")) + return; + /* Delay waiting for the packet master to be turned off + * so that we don't end up back in idle mode because we + * received a packet after boot. + */ + flush(); + ao_delay(AO_SEC_TO_TICKS(1)); + ao_arch_reboot(); + ao_panic(AO_PANIC_REBOOT); +} + +#ifndef HAS_VERSION +#define HAS_VERSION 1 +#endif + +#if HAS_VERSION +static void +version(void) +{ + printf("manufacturer %s\n" + "product %s\n" + "serial-number %u\n" +#if HAS_FLIGHT + "current-flight %u\n" +#endif +#if HAS_LOG + "log-format %u\n" +#endif + , ao_manufacturer + , ao_product + , ao_serial_number +#if HAS_FLIGHT + , ao_flight_number +#endif +#if HAS_LOG + , ao_log_format +#endif + ); + printf("software-version %s\n", ao_version); +} +#endif + +#ifndef NUM_CMDS +#define NUM_CMDS 11 +#endif + +static __code struct ao_cmds *__xdata (ao_cmds[NUM_CMDS]); +static __pdata uint8_t ao_ncmds; + +static void +help(void) +{ + __pdata uint8_t cmds; + __pdata uint8_t cmd; + __code struct ao_cmds * __pdata cs; + __code const char *h; + uint8_t e; + + for (cmds = 0; cmds < ao_ncmds; cmds++) { + cs = ao_cmds[cmds]; + for (cmd = 0; cs[cmd].func; cmd++) { + h = cs[cmd].help; + ao_put_string(h); + e = strlen(h); + h += e + 1; + e = 45 - e; + while (e--) + putchar(' '); + ao_put_string(h); + putchar('\n'); + } + } +} + +static void +report(void) +{ + switch(ao_cmd_status) { + case ao_cmd_lex_error: + case ao_cmd_syntax_error: + puts("Syntax error"); + ao_cmd_status = 0; + default: + break; + } +} + +void +ao_cmd_register(__code struct ao_cmds *cmds) +{ + if (ao_ncmds >= NUM_CMDS) + ao_panic(AO_PANIC_CMD); + ao_cmds[ao_ncmds++] = cmds; +} + +void +ao_cmd(void) +{ + __pdata char c; + uint8_t cmd, cmds; + __code struct ao_cmds * __xdata cs; + void (*__xdata func)(void); + + for (;;) { + readline(); + ao_cmd_lex(); + ao_cmd_white(); + c = ao_cmd_lex_c; + ao_cmd_lex(); + if (c == '\r' || c == '\n') + continue; + func = (void (*)(void)) NULL; + for (cmds = 0; cmds < ao_ncmds; cmds++) { + cs = ao_cmds[cmds]; + for (cmd = 0; cs[cmd].func; cmd++) + if (cs[cmd].help[0] == c) { + func = cs[cmd].func; + break; + } + if (func) + break; + } + if (func) + (*func)(); + else + ao_cmd_status = ao_cmd_syntax_error; + report(); + } +} + +#if HAS_BOOT_LOADER + +#include + +static void +ao_loader(void) +{ + flush(); + ao_boot_loader(); +} +#endif + +__xdata struct ao_task ao_cmd_task; + +__code struct ao_cmds ao_base_cmds[] = { + { help, "?\0Help" }, +#if HAS_TASK_INFO + { ao_task_info, "T\0Tasks" }, +#endif + { echo, "E <0 off, 1 on>\0Echo" }, + { ao_reboot, "r eboot\0Reboot" }, +#if HAS_VERSION + { version, "v\0Version" }, +#endif +#if HAS_BOOT_LOADER + { ao_loader, "X\0Switch to boot loader" }, +#endif + { 0, NULL }, +}; + +void +ao_cmd_init(void) +{ + ao_cmd_register(&ao_base_cmds[0]); + ao_add_task(&ao_cmd_task, ao_cmd, "cmd"); +} diff --git a/src/kernel/ao_companion.h b/src/kernel/ao_companion.h new file mode 100644 index 00000000..035325a3 --- /dev/null +++ b/src/kernel/ao_companion.h @@ -0,0 +1,55 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_COMPANION_H_ +#define _AO_COMPANION_H_ + +/* ao_companion.c */ + +#define AO_COMPANION_SETUP 1 +#define AO_COMPANION_FETCH 2 +#define AO_COMPANION_NOTIFY 3 + +struct ao_companion_command { + uint8_t command; + uint8_t flight_state; + uint16_t tick; + uint16_t serial; + uint16_t flight; + int16_t accel; + int16_t speed; + int16_t height; + int16_t motor_number; +}; + +struct ao_companion_setup { + uint16_t board_id; + uint16_t board_id_inverse; + uint8_t update_period; + uint8_t channels; +}; + +extern __pdata uint8_t ao_companion_running; +extern __xdata uint8_t ao_companion_mutex; +extern __xdata struct ao_companion_command ao_companion_command; +extern __xdata struct ao_companion_setup ao_companion_setup; +extern __xdata uint16_t ao_companion_data[AO_COMPANION_MAX_CHANNELS]; + +void +ao_companion_init(void); + +#endif /* _AO_COMPANION_H_ */ diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c new file mode 100644 index 00000000..4482f673 --- /dev/null +++ b/src/kernel/ao_config.c @@ -0,0 +1,805 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_log.h" +#include +#if HAS_FLIGHT +#include +#include +#endif + +__xdata struct ao_config ao_config; +__pdata uint8_t ao_config_loaded; +__pdata uint8_t ao_config_dirty; +__xdata uint8_t ao_config_mutex; + +#ifndef AO_CONFIG_DEFAULT_APRS_INTERVAL +#define AO_CONFIG_DEFAULT_APRS_INTERVAL 0 +#endif +#define AO_CONFIG_DEFAULT_MAIN_DEPLOY 250 +#define AO_CONFIG_DEFAULT_RADIO_CHANNEL 0 +#define AO_CONFIG_DEFAULT_CALLSIGN "N0CALL" +#define AO_CONFIG_DEFAULT_ACCEL_ZERO_G 16000 +#define AO_CONFIG_DEFAULT_APOGEE_DELAY 0 +#define AO_CONFIG_DEFAULT_IGNITE_MODE AO_IGNITE_MODE_DUAL +#define AO_CONFIG_DEFAULT_PAD_ORIENTATION AO_PAD_ORIENTATION_ANTENNA_UP +#if HAS_EEPROM +#ifndef USE_INTERNAL_FLASH +#error Please define USE_INTERNAL_FLASH +#endif +#endif +#ifndef AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX +#if USE_INTERNAL_FLASH +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ao_storage_config +#else +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 192 * (uint32_t) 1024) +#endif +#endif +#ifndef AO_CONFIG_DEFAULT_RADIO_POWER +#define AO_CONFIG_DEFAULT_RADIO_POWER 0x60 +#endif +#define AO_CONFIG_DEFAULT_RADIO_AMP 0 + +#if HAS_EEPROM +static void +_ao_config_put(void) +{ + ao_config_setup(); + ao_config_erase(); + ao_config_write(0, &ao_config, sizeof (ao_config)); +#if HAS_FLIGHT + ao_log_write_erase(0); +#endif + ao_config_flush(); +} + +void +ao_config_put(void) +{ + ao_mutex_get(&ao_config_mutex); + _ao_config_put(); + ao_mutex_put(&ao_config_mutex); +} +#endif + +#if HAS_RADIO +void +ao_config_set_radio(void) +{ + ao_config.radio_setting = ao_freq_to_set(ao_config.frequency, ao_config.radio_cal); +} +#endif /* HAS_RADIO */ + +static void +_ao_config_get(void) +{ + uint8_t minor; + + if (ao_config_loaded) + return; +#if HAS_EEPROM + /* Yes, I know ao_storage_read calls ao_storage_setup, + * but ao_storage_setup *also* sets ao_storage_config, which we + * need before calling ao_storage_read here + */ + ao_config_setup(); + ao_config_read(0, &ao_config, sizeof (ao_config)); +#endif + if (ao_config.major != AO_CONFIG_MAJOR) { + ao_config.major = AO_CONFIG_MAJOR; + ao_config.minor = 0; + + /* Version 0 stuff */ + ao_config.main_deploy = AO_CONFIG_DEFAULT_MAIN_DEPLOY; + ao_xmemset(&ao_config.callsign, '\0', sizeof (ao_config.callsign)); + ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN), + sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); + ao_config._legacy_radio_channel = 0; + } + minor = ao_config.minor; + if (minor != AO_CONFIG_MINOR) { + /* Fixups for minor version 1 */ + if (minor < 1) + ao_config.apogee_delay = AO_CONFIG_DEFAULT_APOGEE_DELAY; + /* Fixups for minor version 2 */ + if (minor < 2) { + ao_config.accel_plus_g = 0; + ao_config.accel_minus_g = 0; + } + /* Fixups for minor version 3 */ +#if HAS_RADIO + if (minor < 3) + ao_config.radio_cal = ao_radio_cal; +#endif + /* Fixups for minor version 4 */ +#if HAS_FLIGHT + if (minor < 4) + ao_config.flight_log_max = AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX; +#endif + /* Fixupes for minor version 5 */ + if (minor < 5) + ao_config.ignite_mode = AO_CONFIG_DEFAULT_IGNITE_MODE; + if (minor < 6) + ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION; + if (minor < 8) + ao_config.radio_enable = AO_RADIO_ENABLE_CORE; + if (minor < 9) + ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN); + if (minor < 10) + ao_config.frequency = 434550 + ao_config._legacy_radio_channel * 100; + if (minor < 11) + ao_config.apogee_lockout = 0; +#if AO_PYRO_NUM + if (minor < 12) + memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro)); +#endif + if (minor < 13) + ao_config.aprs_interval = AO_CONFIG_DEFAULT_APRS_INTERVAL; +#if HAS_RADIO_POWER + if (minor < 14) + ao_config.radio_power = AO_CONFIG_DEFAULT_RADIO_POWER; + #endif +#if HAS_RADIO_AMP + if (minor < 14) + ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP; +#endif +#if HAS_GYRO + if (minor < 15) { + ao_config.accel_zero_along = 0; + ao_config.accel_zero_across = 0; + ao_config.accel_zero_through = 0; + + /* Reset the main accel offsets to force + * re-calibration + */ + ao_config.accel_plus_g = 0; + ao_config.accel_minus_g = 0; + } +#endif + ao_config.minor = AO_CONFIG_MINOR; + ao_config_dirty = 1; + } +#if HAS_RADIO +#if HAS_FORCE_FREQ + if (ao_force_freq) { + ao_config.frequency = 434550; + ao_config.radio_cal = ao_radio_cal; + ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN), + sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); + } +#endif + ao_config_set_radio(); +#endif + ao_config_loaded = 1; +} + +void +_ao_config_edit_start(void) +{ + ao_mutex_get(&ao_config_mutex); + _ao_config_get(); +} + +void +_ao_config_edit_finish(void) +{ + ao_config_dirty = 1; + ao_mutex_put(&ao_config_mutex); +} + +void +ao_config_get(void) +{ + _ao_config_edit_start(); + ao_mutex_put(&ao_config_mutex); +} + +void +ao_config_callsign_show(void) +{ + printf ("Callsign: \"%s\"\n", ao_config.callsign); +} + +void +ao_config_callsign_set(void) __reentrant +{ + uint8_t c; + static __xdata char callsign[AO_MAX_CALLSIGN + 1]; + + ao_xmemset(callsign, '\0', sizeof callsign); + ao_cmd_white(); + c = 0; + while (ao_cmd_lex_c != '\n') { + if (c < AO_MAX_CALLSIGN) + callsign[c++] = ao_cmd_lex_c; + else + ao_cmd_status = ao_cmd_lex_error; + ao_cmd_lex(); + } + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_xmemcpy(&ao_config.callsign, &callsign, + AO_MAX_CALLSIGN + 1); + _ao_config_edit_finish(); +} + +#if HAS_RADIO + +void +ao_config_frequency_show(void) __reentrant +{ + printf("Frequency: %ld\n", + ao_config.frequency); +} + +void +ao_config_frequency_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.frequency = ao_cmd_lex_u32; + ao_config_set_radio(); + _ao_config_edit_finish(); +#if HAS_RADIO_RECV + ao_radio_recv_abort(); +#endif +} +#endif + +#if HAS_FLIGHT + +void +ao_config_main_deploy_show(void) __reentrant +{ + printf("Main deploy: %d meters\n", + ao_config.main_deploy); +} + +void +ao_config_main_deploy_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.main_deploy = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#if HAS_ACCEL +void +ao_config_accel_calibrate_show(void) __reentrant +{ + printf("Accel cal +1g: %d -1g: %d\n", + ao_config.accel_plus_g, ao_config.accel_minus_g); +#if HAS_GYRO + printf ("IMU cal along %d across %d through %d\n", + ao_config.accel_zero_along, + ao_config.accel_zero_across, + ao_config.accel_zero_through); +#endif +} + +#define ACCEL_CALIBRATE_SAMPLES 1024 +#define ACCEL_CALIBRATE_SHIFT 10 + +#if HAS_GYRO +static int16_t accel_cal_along; +static int16_t accel_cal_across; +static int16_t accel_cal_through; +#endif + +static int16_t +ao_config_accel_calibrate_auto(char *orientation) __reentrant +{ + uint16_t i; + int32_t accel_total; + uint8_t cal_data_ring; +#if HAS_GYRO + int32_t accel_along_total = 0; + int32_t accel_across_total = 0; + int32_t accel_through_total = 0; +#endif + + printf("Orient antenna %s and press a key...", orientation); + flush(); + (void) getchar(); + puts("\r\n"); flush(); + puts("Calibrating..."); flush(); + i = ACCEL_CALIBRATE_SAMPLES; + accel_total = 0; + cal_data_ring = ao_sample_data; + while (i) { + ao_sleep(DATA_TO_XDATA(&ao_sample_data)); + while (i && cal_data_ring != ao_sample_data) { + accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]); +#if HAS_GYRO + accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]); + accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]); + accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]); +#endif + cal_data_ring = ao_data_ring_next(cal_data_ring); + i--; + } + } +#if HAS_GYRO + accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT; + accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT; + accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT; +#endif + return accel_total >> ACCEL_CALIBRATE_SHIFT; +} + +void +ao_config_accel_calibrate_set(void) __reentrant +{ + int16_t up, down; +#if HAS_GYRO + int16_t accel_along_up = 0, accel_along_down = 0; + int16_t accel_across_up = 0, accel_across_down = 0; + int16_t accel_through_up = 0, accel_through_down = 0; +#endif + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + if (ao_cmd_lex_i == 0) { + up = ao_config_accel_calibrate_auto("up"); +#if HAS_GYRO + accel_along_up = accel_cal_along; + accel_across_up = accel_cal_across; + accel_through_up = accel_cal_through; +#endif + down = ao_config_accel_calibrate_auto("down"); +#if HAS_GYRO + accel_along_down = accel_cal_along; + accel_across_down = accel_cal_across; + accel_through_down = accel_cal_through; +#endif + } else { + up = ao_cmd_lex_i; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + down = ao_cmd_lex_i; + } + if (up >= down) { + printf("Invalid accel: up (%d) down (%d)\n", + up, down); + return; + } + _ao_config_edit_start(); + ao_config.accel_plus_g = up; + ao_config.accel_minus_g = down; +#if HAS_GYRO + if (ao_cmd_lex_i == 0) { + ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2; + ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2; + ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2; + } +#endif + _ao_config_edit_finish(); +} +#endif /* HAS_ACCEL */ + +void +ao_config_apogee_delay_show(void) __reentrant +{ + printf("Apogee delay: %d seconds\n", + ao_config.apogee_delay); +} + +void +ao_config_apogee_delay_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.apogee_delay = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +void +ao_config_apogee_lockout_show(void) __reentrant +{ + printf ("Apogee lockout: %d seconds\n", + ao_config.apogee_lockout); +} + +void +ao_config_apogee_lockout_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.apogee_lockout = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif /* HAS_FLIGHT */ + +#if HAS_RADIO +void +ao_config_radio_cal_show(void) __reentrant +{ + printf("Radio cal: %ld\n", ao_config.radio_cal); +} + +void +ao_config_radio_cal_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.radio_cal = ao_cmd_lex_u32; + ao_config_set_radio(); + _ao_config_edit_finish(); +} +#endif + +#if HAS_LOG +void +ao_config_log_show(void) __reentrant +{ + printf("Max flight log: %d kB\n", (int16_t) (ao_config.flight_log_max >> 10)); +} + +void +ao_config_log_set(void) __reentrant +{ + uint16_t block = (uint16_t) (ao_storage_block >> 10); + uint16_t log_max = (uint16_t) (ao_storage_log_max >> 10); + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + if (ao_log_present()) + printf("Storage must be empty before changing log size\n"); + else if (block > 1024 && (ao_cmd_lex_i & (block - 1))) + printf("Flight log size must be multiple of %d kB\n", block); + else if (ao_cmd_lex_i > log_max) + printf("Flight log max %d kB\n", log_max); + else { + _ao_config_edit_start(); + ao_config.flight_log_max = (uint32_t) ao_cmd_lex_i << 10; + _ao_config_edit_finish(); + } +} +#endif /* HAS_LOG */ + +#if HAS_IGNITE +void +ao_config_ignite_mode_show(void) __reentrant +{ + printf("Ignite mode: %d\n", ao_config.ignite_mode); +} + +void +ao_config_ignite_mode_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.ignite_mode = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif + +#if HAS_ACCEL +void +ao_config_pad_orientation_show(void) __reentrant +{ + printf("Pad orientation: %d\n", ao_config.pad_orientation); +} + +#ifndef AO_ACCEL_INVERT +#define AO_ACCEL_INVERT 0x7fff +#endif + +void +ao_config_pad_orientation_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_cmd_lex_i &= 1; + if (ao_config.pad_orientation != ao_cmd_lex_i) { + int16_t t; + t = ao_config.accel_plus_g; + ao_config.accel_plus_g = AO_ACCEL_INVERT - ao_config.accel_minus_g; + ao_config.accel_minus_g = AO_ACCEL_INVERT - t; + } + ao_config.pad_orientation = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif + +#if HAS_RADIO +void +ao_config_radio_enable_show(void) __reentrant +{ + printf("Radio enable: %d\n", ao_config.radio_enable); +} + +void +ao_config_radio_enable_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.radio_enable = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif /* HAS_RADIO */ + +#if HAS_AES + +__xdata uint8_t ao_config_aes_seq = 1; + +void +ao_config_key_show(void) __reentrant +{ + uint8_t i; + printf("AES key: "); + for (i = 0; i < AO_AES_LEN; i++) + printf ("%02x", ao_config.aes_key[i]); + printf("\n"); +} + +void +ao_config_key_set(void) __reentrant +{ + uint8_t i; + + _ao_config_edit_start(); + for (i = 0; i < AO_AES_LEN; i++) { + ao_cmd_hexbyte(); + if (ao_cmd_status != ao_cmd_success) + break; + ao_config.aes_key[i] = ao_cmd_lex_i; + } + ++ao_config_aes_seq; + _ao_config_edit_finish(); +} +#endif + +#if HAS_APRS + +void +ao_config_aprs_show(void) +{ + printf ("APRS interval: %d\n", ao_config.aprs_interval); +} + +void +ao_config_aprs_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.aprs_interval = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif /* HAS_APRS */ + +#if HAS_RADIO_AMP + +void +ao_config_radio_amp_show(void) +{ + printf ("Radio amp setting: %d\n", ao_config.radio_amp); +} + +void +ao_config_radio_amp_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.radio_amp = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif + +#if HAS_RADIO_POWER + +void +ao_config_radio_power_show(void) +{ + printf ("Radio power setting: %d\n", ao_config.radio_power); +} + +void +ao_config_radio_power_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.radio_power = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif + +struct ao_config_var { + __code char *str; + void (*set)(void) __reentrant; + void (*show)(void) __reentrant; +}; + +static void +ao_config_help(void) __reentrant; + +static void +ao_config_show(void) __reentrant; + +#if HAS_EEPROM +static void +ao_config_save(void) __reentrant; +#endif + +__code struct ao_config_var ao_config_vars[] = { +#if HAS_FLIGHT + { "m \0Main deploy (m)", + ao_config_main_deploy_set, ao_config_main_deploy_show, }, + { "d \0Apogee delay (s)", + ao_config_apogee_delay_set, ao_config_apogee_delay_show }, + { "L \0Apogee detect lockout (s)", + ao_config_apogee_lockout_set, ao_config_apogee_lockout_show, }, +#endif /* HAS_FLIGHT */ +#if HAS_RADIO + { "F \0Frequency (kHz)", + ao_config_frequency_set, ao_config_frequency_show }, + { "c \0Callsign (8 char max)", + ao_config_callsign_set, ao_config_callsign_show }, + { "e <0 disable, 1 enable>\0Enable telemetry and RDF", + ao_config_radio_enable_set, ao_config_radio_enable_show }, + { "f \0Radio calib (cal = rf/(xtal/2^16))", + ao_config_radio_cal_set, ao_config_radio_cal_show }, +#if HAS_RADIO_POWER + { "p \0Radio power setting (0-255)", + ao_config_radio_power_set, ao_config_radio_power_show }, +#endif +#if HAS_RADIO_AMP + { "d \0Radio amplifier setting (0-3)", + ao_config_radio_amp_set, ao_config_radio_amp_show }, +#endif +#endif /* HAS_RADIO */ +#if HAS_ACCEL + { "a <+g> <-g>\0Accel calib (0 for auto)", + ao_config_accel_calibrate_set,ao_config_accel_calibrate_show }, + { "o <0 antenna up, 1 antenna down>\0Set pad orientation", + ao_config_pad_orientation_set,ao_config_pad_orientation_show }, +#endif /* HAS_ACCEL */ +#if HAS_LOG + { "l \0Flight log size (kB)", + ao_config_log_set, ao_config_log_show }, +#endif +#if HAS_IGNITE + { "i <0 dual, 1 apogee, 2 main>\0Set igniter mode", + ao_config_ignite_mode_set, ao_config_ignite_mode_show }, +#endif +#if HAS_AES + { "k <32 hex digits>\0Set AES encryption key", + ao_config_key_set, ao_config_key_show }, +#endif +#if AO_PYRO_NUM + { "P \0Configure pyro channels", + ao_pyro_set, ao_pyro_show }, +#endif +#if HAS_APRS + { "A \0APRS packet interval (0 disable)", + ao_config_aprs_set, ao_config_aprs_show }, +#endif + { "s\0Show", + ao_config_show, 0 }, +#if HAS_EEPROM + { "w\0Write to eeprom", + ao_config_save, 0 }, +#endif + { "?\0Help", + ao_config_help, 0 }, + { 0, 0, 0 } +}; + +void +ao_config_set(void) +{ + char c; + uint8_t cmd; + + ao_cmd_white(); + c = ao_cmd_lex_c; + ao_cmd_lex(); + for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) + if (ao_config_vars[cmd].str[0] == c) { + (*ao_config_vars[cmd].set)(); + return; + } + ao_cmd_status = ao_cmd_syntax_error; +} + +static void +ao_config_help(void) __reentrant +{ + uint8_t cmd; + for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) + printf("%-20s %s\n", + ao_config_vars[cmd].str, + ao_config_vars[cmd].str+1+ + strlen(ao_config_vars[cmd].str)); +} + +static void +ao_config_show(void) __reentrant +{ + uint8_t cmd; + ao_config_get(); + printf("Config version: %d.%d\n", + ao_config.major, ao_config.minor); + for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) + if (ao_config_vars[cmd].show) + (*ao_config_vars[cmd].show)(); +#if HAS_MS5607 + ao_ms5607_info(); +#endif +} + +#if HAS_EEPROM +static void +ao_config_save(void) __reentrant +{ + uint8_t saved = 0; + ao_mutex_get(&ao_config_mutex); + if (ao_config_dirty) { + _ao_config_put(); + ao_config_dirty = 0; + saved = 1; + } + ao_mutex_put(&ao_config_mutex); + if (saved) + puts("Saved"); + else + puts("Nothing to save"); +} +#endif + +__code struct ao_cmds ao_config_cmds[] = { + { ao_config_set, "c \0Set config (? for help, s to show)" }, + { 0, NULL }, +}; + +void +ao_config_init(void) +{ + ao_cmd_register(&ao_config_cmds[0]); +} diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h new file mode 100644 index 00000000..e101af8e --- /dev/null +++ b/src/kernel/ao_config.h @@ -0,0 +1,53 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_CONFIG_H_ +#define _AO_CONFIG_H_ + +#ifndef USE_STORAGE_CONFIG +#define USE_STORAGE_CONFIG 1 +#endif + +#ifndef USE_EEPROM_CONFIG +#define USE_EEPROM_CONFIG 0 +#endif + +#if USE_STORAGE_CONFIG + +#include + +#define ao_config_setup() ao_storage_setup() +#define ao_config_erase() ao_storage_erase(ao_storage_config) +#define ao_config_write(pos,bytes, len) ao_storage_write(ao_storage_config+(pos), bytes, len) +#define ao_config_read(pos,bytes, len) ao_storage_read(ao_storage_config+(pos), bytes, len) +#define ao_config_flush() ao_storage_flush() + +#endif + +#if USE_EEPROM_CONFIG + +#include + +#define ao_config_setup() +#define ao_config_erase() +#define ao_config_write(pos,bytes, len) ao_eeprom_write(pos, bytes, len) +#define ao_config_read(pos,bytes, len) ao_eeprom_read(pos, bytes, len) +#define ao_config_flush() + +#endif + +#endif /* _AO_CONFIG_H_ */ diff --git a/src/kernel/ao_convert.c b/src/kernel/ao_convert.c new file mode 100644 index 00000000..aa9b5f48 --- /dev/null +++ b/src/kernel/ao_convert.c @@ -0,0 +1,89 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST) +#include "ao.h" +#endif + +static const int16_t altitude_table[] = { +#include "altitude.h" +}; + +#define ALT_FRAC_SCALE (1 << ALT_FRAC_BITS) +#define ALT_FRAC_MASK (ALT_FRAC_SCALE - 1) + +int16_t +ao_pres_to_altitude(int16_t pres) __reentrant +{ + uint8_t o; + int16_t part; + + if (pres < 0) + pres = 0; + o = pres >> ALT_FRAC_BITS; + part = pres & ALT_FRAC_MASK; + + return ((int32_t) altitude_table[o] * (ALT_FRAC_SCALE - part) + + (int32_t) altitude_table[o+1] * part + (ALT_FRAC_SCALE >> 1)) >> ALT_FRAC_BITS; +} + +#if AO_NEED_ALTITUDE_TO_PRES +int16_t +ao_altitude_to_pres(int16_t alt) __reentrant +{ + int16_t span, sub_span; + uint8_t l, h, m; + int32_t pres; + + l = 0; + h = NALT - 1; + while ((h - l) != 1) { + m = (l + h) >> 1; + if (altitude_table[m] < alt) + h = m; + else + l = m; + } + span = altitude_table[l] - altitude_table[h]; + sub_span = altitude_table[l] - alt; + pres = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_FRAC_BITS) + (span >> 1)) / span; + if (pres > 32767) + pres = 32767; + if (pres < 0) + pres = 0; + return (int16_t) pres; +} +#endif + +#if 0 +int16_t +ao_temp_to_dC(int16_t temp) __reentrant +{ + int16_t ret; + + /* Output voltage at 0°C = 0.755V + * Coefficient = 0.00247V/°C + * Reference voltage = 1.25V + * + * temp = ((value / 32767) * 1.25 - 0.755) / 0.00247 + * = (value - 19791.268) / 32768 * 1.25 / 0.00247 + * ≃ (value - 19791) * 1012 / 65536 + */ + ret = ((temp - 19791) * 1012L) >> 16; + return ret; +} +#endif diff --git a/src/kernel/ao_convert_pa.c b/src/kernel/ao_convert_pa.c new file mode 100644 index 00000000..fe6e0ef6 --- /dev/null +++ b/src/kernel/ao_convert_pa.c @@ -0,0 +1,82 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST) +#include "ao.h" +#endif + +#ifndef AO_CONST_ATTRIB +#define AO_CONST_ATTRIB +#endif + +static const alt_t altitude_table[] AO_CONST_ATTRIB = { +#include "altitude-pa.h" +}; + +#ifndef FETCH_ALT +#define FETCH_ALT(o) altitude_table[o] +#endif + +#define ALT_SCALE (1 << ALT_SHIFT) +#define ALT_MASK (ALT_SCALE - 1) + +alt_t +ao_pa_to_altitude(int32_t pa) +{ + int16_t o; + int16_t part; + int32_t low, high; + + if (pa < 0) + pa = 0; + if (pa > 120000L) + pa = 120000L; + o = pa >> ALT_SHIFT; + part = pa & ALT_MASK; + + low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part); + high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); + return (low + high) >> ALT_SHIFT; +} + +#ifdef AO_CONVERT_TEST +int32_t +ao_altitude_to_pa(int32_t alt) +{ + int32_t span, sub_span; + uint16_t l, h, m; + int32_t pa; + + l = 0; + h = NALT - 1; + while ((h - l) != 1) { + m = (l + h) >> 1; + if (altitude_table[m] < alt) + h = m; + else + l = m; + } + span = altitude_table[l] - altitude_table[h]; + sub_span = altitude_table[l] - alt; + pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span; + if (pa > 120000) + pa = 120000; + if (pa < 0) + pa = 0; + return pa; +} +#endif diff --git a/src/kernel/ao_convert_pa_test.c b/src/kernel/ao_convert_pa_test.c new file mode 100644 index 00000000..7d5b1922 --- /dev/null +++ b/src/kernel/ao_convert_pa_test.c @@ -0,0 +1,76 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#define AO_CONVERT_TEST +typedef int32_t alt_t; +#include "ao_host.h" +#include "ao_convert_pa.c" + +#define STEP_P 1 +#define STEP_A 1 + +static inline int i_abs(int i) { return i < 0 ? -i : i; } + +int +main (int argc, char **argv) +{ + int i; + int32_t p_to_a, p_to_a_to_p; + int32_t a_to_p, a_to_p_to_a; + int max_p_error = 0, max_p_error_p = -1; + int max_a_error = 0, max_a_error_a = -1; + int p_error; + int a_error; + int ret = 0; + + for (i = 0; i < 120000 + STEP_P; i += STEP_P) { + if (i > 120000) + i = 120000; + p_to_a = ao_pa_to_altitude(i); + p_to_a_to_p = ao_altitude_to_pa(p_to_a); + p_error = i_abs(p_to_a_to_p - i); + if (p_error > max_p_error) { + max_p_error = p_error; + max_p_error_p = i; + } +// printf ("pa %d alt %d pa %d\n", +// i, p_to_a, p_to_a_to_p); + } + for (i = -1450; i < 40000 + STEP_A; i += STEP_A) { + a_to_p = ao_altitude_to_pa(i); + a_to_p_to_a = ao_pa_to_altitude(a_to_p); + a_error = i_abs(a_to_p_to_a - i); + if (a_error > max_a_error) { + max_a_error = a_error; + max_a_error_a = i; + } +// printf ("alt %d pa %d alt %d\n", +// i, a_to_p, a_to_p_to_a); + } + if (max_p_error > 2) { + printf ("max p error %d at %d\n", max_p_error, + max_p_error_p); + ret++; + } + if (max_a_error > 1) { + printf ("max a error %d at %d\n", max_a_error, + max_a_error_a); + ret++; + } + return ret; +} diff --git a/src/kernel/ao_convert_test.c b/src/kernel/ao_convert_test.c new file mode 100644 index 00000000..87e76841 --- /dev/null +++ b/src/kernel/ao_convert_test.c @@ -0,0 +1,76 @@ +/* + * Copyright © 2010 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#define AO_CONVERT_TEST +#define AO_NEED_ALTITUDE_TO_PRES 1 +#include "ao_host.h" +#include "ao_convert.c" + +#define STEP 1 + +static inline int i_abs(int i) { return i < 0 ? -i : i; } + +int main (int argc, char **argv) +{ + int i; + int16_t p_to_a, p_to_a_to_p; + int16_t a_to_p, a_to_p_to_a; + int max_p_error = 0, max_p_error_p = -1; + int max_a_error = 0, max_a_error_a = -1; + int p_error; + int a_error; + int ret = 0; + + for (i = 0; i < 32767 + STEP; i += STEP) { + if (i > 32767) + i = 32767; + p_to_a = ao_pres_to_altitude(i); + p_to_a_to_p = ao_altitude_to_pres(p_to_a); + p_error = i_abs(p_to_a_to_p - i); + if (p_error > max_p_error) { + max_p_error = p_error; + max_p_error_p = i; + } +// printf ("pres %d alt %d pres %d\n", +// i, p_to_a, p_to_a_to_p); + } + for (i = -1578; i < 15835 + STEP; i += STEP) { + if (i > 15835) + i = 15835; + a_to_p = ao_altitude_to_pres(i); + a_to_p_to_a = ao_pres_to_altitude(a_to_p); + a_error = i_abs(a_to_p_to_a - i); + if (a_error > max_a_error) { + max_a_error = a_error; + max_a_error_a = i; + } +// printf ("alt %d pres %d alt %d\n", +// i, a_to_p, a_to_p_to_a); + } + if (max_p_error > 2) { + printf ("max p error %d at %d\n", max_p_error, + max_p_error_p); + ret++; + } + if (max_a_error > 1) { + printf ("max a error %d at %d\n", max_a_error, + max_a_error_a); + ret++; + } + return ret; +} diff --git a/src/kernel/ao_convert_volt.c b/src/kernel/ao_convert_volt.c new file mode 100644 index 00000000..8556d423 --- /dev/null +++ b/src/kernel/ao_convert_volt.c @@ -0,0 +1,33 @@ +/* + * Copyright © 2014 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +#define scale(v,p,m) ((int32_t) (v) * (AO_ADC_REFERENCE_DV * ((p) + (m))) / (AO_ADC_MAX * (m))) + +int16_t +ao_battery_decivolt(int16_t adc) +{ + return scale(adc, AO_BATTERY_DIV_PLUS, AO_BATTERY_DIV_MINUS); +} + +int16_t +ao_ignite_decivolt(int16_t adc) +{ + return scale(adc, AO_IGNITE_DIV_PLUS, AO_IGNITE_DIV_MINUS); +} + diff --git a/src/kernel/ao_data.c b/src/kernel/ao_data.c new file mode 100644 index 00000000..6a3d02a1 --- /dev/null +++ b/src/kernel/ao_data.c @@ -0,0 +1,36 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; +volatile __data uint8_t ao_data_head; +volatile __data uint8_t ao_data_present; + +#ifndef ao_data_count +void +ao_data_get(__xdata struct ao_data *packet) +{ +#if HAS_FLIGHT + uint8_t i = ao_data_ring_prev(ao_sample_data); +#else + uint8_t i = ao_data_ring_prev(ao_data_head); +#endif + memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data)); +} +#endif diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h new file mode 100644 index 00000000..c4b062fd --- /dev/null +++ b/src/kernel/ao_data.h @@ -0,0 +1,338 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_DATA_H_ +#define _AO_DATA_H_ + +#define GRAVITY 9.80665 + +#if HAS_ADC +#define AO_DATA_ADC (1 << 0) +#else +#define AO_DATA_ADC 0 +#endif + +#if HAS_MS5607 +#include +#define AO_DATA_MS5607 (1 << 1) +#else +#define AO_DATA_MS5607 0 +#endif + +#if HAS_MPU6000 +#include +#define AO_DATA_MPU6000 (1 << 2) +#else +#define AO_DATA_MPU6000 0 +#endif + +#if HAS_HMC5883 +#include +#define AO_DATA_HMC5883 (1 << 3) +#else +#define AO_DATA_HMC5883 0 +#endif + +#if HAS_MMA655X +#include +#define AO_DATA_MMA655X (1 << 4) +#else +#define AO_DATA_MMA655X 0 +#endif + +#ifdef AO_DATA_RING + +#define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X) + +struct ao_data { + uint16_t tick; +#if HAS_ADC + struct ao_adc adc; +#endif +#if HAS_MS5607 + struct ao_ms5607_sample ms5607_raw; + struct ao_ms5607_value ms5607_cooked; +#endif +#if HAS_MPU6000 + struct ao_mpu6000_sample mpu6000; +#if !HAS_MMA655X + int16_t z_accel; +#endif +#endif +#if HAS_HMC5883 + struct ao_hmc5883_sample hmc5883; +#endif +#if HAS_MMA655X + uint16_t mma655x; +#endif +}; + +#define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) +#define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) + +/* Get a copy of the last complete sample set */ +void +ao_data_get(__xdata struct ao_data *packet); + +extern volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; +extern volatile __data uint8_t ao_data_head; +extern volatile __data uint8_t ao_data_present; +extern volatile __data uint8_t ao_data_count; + +/* + * Mark a section of data as ready, check for data complete + */ +#define AO_DATA_PRESENT(bit) (ao_data_present |= (bit)) + +/* + * Wait until it is time to write a sensor sample; this is + * signaled by the timer tick + */ +#define AO_DATA_WAIT() do { \ + ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count)); \ + } while (0) + +#endif /* AO_DATA_RING */ + +#if !HAS_BARO && HAS_MS5607 + +/* Either an MS5607 or an MS5611 hooked to a SPI port + */ + +#define HAS_BARO 1 + +typedef int32_t pres_t; + +#ifndef AO_ALT_TYPE +#define AO_ALT_TYPE int32_t +#endif + +typedef AO_ALT_TYPE alt_t; + +#define ao_data_pres_cook(packet) ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked) + +#define ao_data_pres(packet) ((packet)->ms5607_cooked.pres) +#define ao_data_temp(packet) ((packet)->ms5607_cooked.temp) + +#define pres_to_altitude(p) ao_pa_to_altitude(p) + +#endif + +#if !HAS_BARO && HAS_ADC + +#define HAS_BARO 1 + +typedef int16_t pres_t; +typedef int16_t alt_t; + +#define ao_data_pres(packet) ((packet)->adc.pres) +#define ao_data_temp(packet) ((packet)->adc.temp) +#define pres_to_altitude(p) ao_pres_to_altitude(p) +#define ao_data_pres_cook(p) + +#endif + +#if !HAS_BARO +typedef int16_t alt_t; +#endif + +/* + * Need a few macros to pull data from the sensors: + * + * ao_data_accel_sample - pull raw sensor and convert to normalized values + * ao_data_accel - pull normalized value (lives in the same memory) + * ao_data_set_accel - store normalized value back in the sensor location + * ao_data_accel_invert - flip rocket ends for positive acceleration + */ + +#if HAS_ACCEL + +/* This section is for an analog accelerometer hooked to one of the ADC pins. As + * those are 5V parts, this also requires that the 5V supply be hooked to to anothe ADC + * pin so that the both can be measured to correct for changes between the 3.3V and 5V rails + */ + +typedef int16_t accel_t; +#define ao_data_accel(packet) ((packet)->adc.accel) +#define ao_data_set_accel(packet, a) ((packet)->adc.accel = (a)) +#define ao_data_accel_invert(a) (0x7fff -(a)) + +/* + * Ok, the math here is a bit tricky. + * + * ao_sample_accel: ADC output for acceleration + * ao_accel_ref: ADC output for the 5V reference. + * ao_cook_accel: Corrected acceleration value + * Vcc: 3.3V supply to the CC1111 + * Vac: 5V supply to the accelerometer + * accel: input voltage to accelerometer ADC pin + * ref: input voltage to 5V reference ADC pin + * + * + * Measured acceleration is ratiometric to Vcc: + * + * ao_sample_accel accel + * ------------ = ----- + * 32767 Vcc + * + * Measured 5v reference is also ratiometric to Vcc: + * + * ao_accel_ref ref + * ------------ = ----- + * 32767 Vcc + * + * + * ao_accel_ref = 32767 * (ref / Vcc) + * + * Acceleration is measured ratiometric to the 5V supply, + * so what we want is: + * + * ao_cook_accel accel + * ------------- = ----- + * 32767 ref + * + * + * accel Vcc + * = ----- * --- + * Vcc ref + * + * ao_sample_accel 32767 + * = ------------ * ------------ + * 32767 ao_accel_ref + * + * Multiply through by 32767: + * + * ao_sample_accel * 32767 + * ao_cook_accel = -------------------- + * ao_accel_ref + * + * Now, the tricky part. Getting this to compile efficiently + * and keeping all of the values in-range. + * + * First off, we need to use a shift of 16 instead of * 32767 as SDCC + * does the obvious optimizations for byte-granularity shifts: + * + * ao_cook_accel = (ao_sample_accel << 16) / ao_accel_ref + * + * Next, lets check our input ranges: + * + * 0 <= ao_sample_accel <= 0x7fff (singled ended ADC conversion) + * 0x7000 <= ao_accel_ref <= 0x7fff (the 5V ref value is close to 0x7fff) + * + * Plugging in our input ranges, we get an output range of 0 - 0x12490, + * which is 17 bits. That won't work. If we take the accel ref and shift + * by a bit, we'll change its range: + * + * 0xe000 <= ao_accel_ref<<1 <= 0xfffe + * + * ao_cook_accel = (ao_sample_accel << 16) / (ao_accel_ref << 1) + * + * Now the output range is 0 - 0x9248, which nicely fits in 16 bits. It + * is, however, one bit too large for our signed computations. So, we + * take the result and shift that by a bit: + * + * ao_cook_accel = ((ao_sample_accel << 16) / (ao_accel_ref << 1)) >> 1 + * + * This finally creates an output range of 0 - 0x4924. As the ADC only + * provides 11 bits of data, we haven't actually lost any precision, + * just dropped a bit of noise off the low end. + */ + +#if HAS_ACCEL_REF + +#define ao_data_accel_cook(packet) \ + ((uint16_t) ((((uint32_t) (packet)->adc.accel << 16) / ((packet)->adc.accel_ref << 1))) >> 1) + +#else + +#define ao_data_accel_cook(packet) ((packet)->adc.accel) + +#endif /* HAS_ACCEL_REF */ + +#endif /* HAS_ACCEL */ + +#if !HAS_ACCEL && HAS_MMA655X + +#define HAS_ACCEL 1 + +typedef int16_t accel_t; + +/* MMA655X is hooked up so that positive values represent negative acceleration */ + +#define AO_ACCEL_INVERT 4095 + +#define ao_data_accel(packet) ((packet)->mma655x) +#if AO_MMA655X_INVERT +#define ao_data_accel_cook(packet) (AO_ACCEL_INVERT - (packet)->mma655x) +#else +#define ao_data_accel_cook(packet) ((packet)->mma655x) +#endif +#define ao_data_set_accel(packet, accel) ((packet)->mma655x = (accel)) +#define ao_data_accel_invert(accel) (AO_ACCEL_INVERT - (accel)) + +#endif + +#if !HAS_ACCEL && HAS_MPU6000 + +#define HAS_ACCEL 1 + +#define AO_ACCEL_INVERT 0 + +typedef int16_t accel_t; + +/* MPU6000 is hooked up so that positive y is positive acceleration */ +#define ao_data_accel(packet) ((packet)->z_accel) +#define ao_data_accel_cook(packet) (-(packet)->mpu6000.accel_y) +#define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) +#define ao_data_accel_invert(a) (-(a)) + +#endif + +#if !HAS_GYRO && HAS_MPU6000 + +#define HAS_GYRO 1 + +typedef int16_t gyro_t; /* in raw sample units */ +typedef int16_t angle_t; /* in degrees */ + +/* Y axis is aligned with the direction of motion (along) */ +/* X axis is aligned in the other board axis (across) */ +/* Z axis is aligned perpendicular to the board (through) */ + +#define ao_data_along(packet) ((packet)->mpu6000.accel_y) +#define ao_data_across(packet) ((packet)->mpu6000.accel_x) +#define ao_data_through(packet) ((packet)->mpu6000.accel_z) + +#define ao_data_roll(packet) ((packet)->mpu6000.gyro_y) +#define ao_data_pitch(packet) ((packet)->mpu6000.gyro_x) +#define ao_data_yaw(packet) ((packet)->mpu6000.gyro_z) + +#endif + +#if !HAS_MAG && HAS_HMC5883 + +#define HAS_MAG 1 + +typedef int16_t ao_mag_t; /* in raw sample units */ + +#define ao_data_mag_along(packet) ((packet)->hmc5883.x) +#define ao_data_mag_across(packet) ((packet)->hmc5883.y) +#define ao_data_mag_through(packet) ((packet)->hmc5883.z) + +#endif + +#endif /* _AO_DATA_H_ */ diff --git a/src/kernel/ao_dbg.h b/src/kernel/ao_dbg.h new file mode 100644 index 00000000..181e6ec2 --- /dev/null +++ b/src/kernel/ao_dbg.h @@ -0,0 +1,62 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_DBG_H_ +#define _AO_DBG_H_ + +/* + * ao_dbg.c + * + * debug another telemetrum board + */ + +/* Send a byte to the dbg target */ +void +ao_dbg_send_byte(uint8_t byte); + +/* Receive a byte from the dbg target */ +uint8_t +ao_dbg_recv_byte(void); + +/* Start a bulk transfer to/from dbg target memory */ +void +ao_dbg_start_transfer(uint16_t addr); + +/* End a bulk transfer to/from dbg target memory */ +void +ao_dbg_end_transfer(void); + +/* Write a byte to dbg target memory */ +void +ao_dbg_write_byte(uint8_t byte); + +/* Read a byte from dbg target memory */ +uint8_t +ao_dbg_read_byte(void); + +/* Enable dbg mode, switching use of the pins */ +void +ao_dbg_debug_mode(void); + +/* Reset the dbg target */ +void +ao_dbg_reset(void); + +void +ao_dbg_init(void); + +#endif /* _AO_DBG_H_ */ diff --git a/src/kernel/ao_debounce.c b/src/kernel/ao_debounce.c new file mode 100644 index 00000000..b9d67729 --- /dev/null +++ b/src/kernel/ao_debounce.c @@ -0,0 +1,163 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include + +static uint8_t ao_debounce_initialized; +static uint8_t ao_debounce_running; +static struct ao_debounce *ao_debounce; + +static uint8_t values[64]; +static uint8_t n; + +#define d_step(n) (((n) + 1) & 63) + +static void +_ao_debounce_set(struct ao_debounce *debounce, uint8_t value) +{ + if (value != debounce->value) { + values[n] = value; + n = (n + 1) & 63; + debounce->value = value; + debounce->_set(debounce, value); + } + _ao_debounce_stop(debounce); +} + +void +ao_debounce_dump(void) +{ + uint8_t s; + + for (s = 0; s < n; s++) { + printf ("%d: %d\n", + s, values[s]); + } + n = 0; +} + +/* + * Get the current value, set the result when we've + * reached the debounce count limit + */ +static void +_ao_debounce_check(struct ao_debounce *debounce) +{ + uint8_t next = debounce->_get(debounce); + + if (next == debounce->current) { + if (debounce->count < debounce->hold) { + if (++debounce->count == debounce->hold) + _ao_debounce_set(debounce, debounce->current); + } + } else { + debounce->count = 0; + debounce->current = next; + } +} + +static void +_ao_debounce_isr(void) +{ + struct ao_debounce *debounce, *next; + + for (debounce = ao_debounce; debounce; debounce = next) { + next = debounce->next; + _ao_debounce_check(debounce); + } +} + +static void +ao_debounce_on(void) +{ + ao_fast_timer_on(_ao_debounce_isr); +} + +static void +ao_debounce_off(void) +{ + ao_fast_timer_off(_ao_debounce_isr); +} + +/* + * Start monitoring one pin + */ +void +_ao_debounce_start(struct ao_debounce *debounce) +{ + uint32_t m; + + m = ao_arch_irqsave(); + if (!debounce->running) { + debounce->running = 1; + + /* Reset the counter */ + debounce->count = 0; + + /* Link into list */ + debounce->next = ao_debounce; + ao_debounce = debounce; + + /* Make sure the timer is running */ + if (!ao_debounce_running++) + ao_debounce_on(); + + /* And go check the current value */ + _ao_debounce_check(debounce); + } + ao_arch_irqrestore(m); +} + +/* + * Stop monitoring one pin + */ +void +_ao_debounce_stop(struct ao_debounce *debounce) +{ + struct ao_debounce **prev; + uint32_t m; + + m = ao_arch_irqsave(); + if (debounce->running) { + debounce->running = 0; + + /* Unlink */ + for (prev = &ao_debounce; (*prev); prev = &((*prev)->next)) { + if (*prev == debounce) { + *prev = debounce->next; + break; + } + } + debounce->next = NULL; + + /* Turn off the timer if possible */ + if (!--ao_debounce_running) + ao_debounce_off(); + } + ao_arch_irqrestore(m); +} + +void +ao_debounce_init(void) +{ + if (ao_debounce_initialized) + return; + ao_debounce_initialized = 1; + ao_fast_timer_init(); +} diff --git a/src/kernel/ao_debounce.h b/src/kernel/ao_debounce.h new file mode 100644 index 00000000..19c620f5 --- /dev/null +++ b/src/kernel/ao_debounce.h @@ -0,0 +1,74 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_DEBOUNCE_H_ +#define _AO_DEBOUNCE_H_ + +struct ao_debounce { + struct ao_debounce *next; + + /* time that pin value must be stable before accepting */ + uint8_t hold; + + /* last value reported to app; don't report it twice */ + uint8_t value; + + /* current value received from pins */ + uint8_t current; + + /* current count of intervals pin value has been stable */ + uint8_t count; + + /* This pin is running */ + uint8_t running; + + /* Get the current pin value */ + uint8_t (*_get)(struct ao_debounce *debounce); + + /* The stable value has changed */ + void (*_set)(struct ao_debounce *debounce, uint8_t value); +}; + +static inline void +ao_debounce_config(struct ao_debounce *debounce, + uint8_t (*_get)(struct ao_debounce *debounce), + void (*_set)(struct ao_debounce *debounce, uint8_t value), + uint8_t hold) +{ + debounce->next = 0; + debounce->hold = hold; + debounce->value = 0xff; + debounce->current = 0xff; + debounce->count = 0; + debounce->running = 0; + debounce->_get = _get; + debounce->_set = _set; +} + +void +_ao_debounce_start(struct ao_debounce *debounce); + +void +_ao_debounce_stop(struct ao_debounce *debounce); + +void +ao_debounce_init(void); + +void +ao_debounce_dump(void); + +#endif /* _AO_DEBOUNCE_H_ */ diff --git a/src/kernel/ao_ee_fake.c b/src/kernel/ao_ee_fake.c new file mode 100644 index 00000000..7fcfcab0 --- /dev/null +++ b/src/kernel/ao_ee_fake.c @@ -0,0 +1,37 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +/* + * For hardware without eeprom, the config code still + * wants to call these functions + */ +uint8_t +ao_ee_write_config(uint8_t *buf, uint16_t len) __reentrant +{ + (void) buf; + (void) len; + return 1; +} + +uint8_t +ao_ee_read_config(uint8_t *buf, uint16_t len) __reentrant +{ + ao_xmemset(buf, '\0', len); + return 1; +} diff --git a/src/kernel/ao_eeprom.h b/src/kernel/ao_eeprom.h new file mode 100644 index 00000000..915522bf --- /dev/null +++ b/src/kernel/ao_eeprom.h @@ -0,0 +1,43 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_EEPROM_H_ +#define _AO_EEPROM_H_ + +extern const ao_pos_t ao_eeprom_total; + +/* + * Write to eeprom + */ + +uint8_t +ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len); + +/* + * Read from eeprom + */ +uint8_t +ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len); + +/* + * Initialize eeprom + */ + +void +ao_eeprom_init(void); + +#endif /* _AO_EEPROM_H_ */ diff --git a/src/kernel/ao_fec.h b/src/kernel/ao_fec.h new file mode 100644 index 00000000..618756c1 --- /dev/null +++ b/src/kernel/ao_fec.h @@ -0,0 +1,84 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_FEC_H_ +#define _AO_FEC_H_ + +#include + +#define AO_FEC_CRC_INIT 0xffff +#define AO_FEC_TRELLIS_TERMINATOR 0x0b +#define AO_FEC_PREPARE_EXTRA 4 + +extern const uint8_t ao_fec_whiten_table[]; + +#if AO_FEC_DEBUG +void +ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name); +#endif + +static inline uint16_t +ao_fec_crc_byte(uint8_t byte, uint16_t crc) +{ + uint8_t bit; + + for (bit = 0; bit < 8; bit++) { + if (((crc & 0x8000) >> 8) ^ (byte & 0x80)) + crc = (crc << 1) ^ 0x8005; + else + crc = (crc << 1); + byte <<= 1; + } + return crc; +} + +uint16_t +ao_fec_crc(const uint8_t *bytes, uint8_t len); + +/* + * 'len' is the length of the original data; 'bytes' + * must be four bytes longer than that, and the first + * two after 'len' must be the received crc + */ +uint8_t +ao_fec_check_crc(const uint8_t *bytes, uint8_t len); + +/* + * Compute CRC, whiten, convolve and interleave data. 'out' must be (len + 4) * 2 bytes long + */ +uint8_t +ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out); + +/* + * Decode data. 'in' is one byte per bit, soft decision + * 'out' must be len/8 bytes long + */ + +#define AO_FEC_DECODE_BLOCK (32) /* callback must return multiples of this many bits */ + +#define AO_FEC_DECODE_CRC_OK 0x80 /* stored in out[out_len-1] */ + +uint8_t +ao_fec_decode(const uint8_t *in, uint16_t in_len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void)); + +/* + * Interleave data packed in bytes. 'out' must be 'len' bytes long. + */ +uint16_t +ao_fec_interleave_bytes(uint8_t *in, uint16_t len, uint8_t *out); + +#endif /* _AO_FEC_H_ */ diff --git a/src/kernel/ao_fec_rx.c b/src/kernel/ao_fec_rx.c new file mode 100644 index 00000000..c4f5559a --- /dev/null +++ b/src/kernel/ao_fec_rx.c @@ -0,0 +1,318 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +#ifdef TELEMEGA +#include +#endif + +#if AO_PROFILE +#include + +uint32_t ao_fec_decode_start, ao_fec_decode_end; +#endif + +/* + * byte order repeats through 3 2 1 0 + * + * bit-pair order repeats through + * + * 1/0 3/2 5/4 7/6 + * + * So, the over all order is: + * + * 3,1/0 2,1/0 1,1/0 0,1/0 + * 3,3/2 2,3/2 1,3/2 0,3/2 + * 3,5/4 2,5/4 1,5/4 0,5/4 + * 3,7/6 2,7/6 1,7/6 0,7/6 + * + * The raw bit order is thus + * + * 1e/1f 16/17 0e/0f 06/07 + * 1c/1d 14/15 0c/0d 04/05 + * 1a/1b 12/13 0a/0b 02/03 + * 18/19 10/11 08/09 00/01 + */ + +static const uint8_t ao_interleave_order[] = { + 0x1e, 0x16, 0x0e, 0x06, + 0x1c, 0x14, 0x0c, 0x04, + 0x1a, 0x12, 0x0a, 0x02, + 0x18, 0x10, 0x08, 0x00 +}; + +static inline uint16_t ao_interleave_index(uint16_t i) { + return (i & ~0x1e) | ao_interleave_order[(i & 0x1e) >> 1]; +} + +#define NUM_STATE 8 +#define NUM_HIST 24 + +typedef uint32_t bits_t; + +#define V_0 0xff +#define V_1 0x00 + +/* + * These are just the 'zero' states; the 'one' states mirror them + */ +static const uint8_t ao_fec_decode_table[NUM_STATE*2] = { + V_0, V_0, /* 000 */ + V_0, V_1, /* 001 */ + V_1, V_1, /* 010 */ + V_1, V_0, /* 011 */ + V_1, V_1, /* 100 */ + V_1, V_0, /* 101 */ + V_0, V_0, /* 110 */ + V_0, V_1 /* 111 */ +}; + +static inline uint8_t +ao_next_state(uint8_t state, uint8_t bit) +{ + return ((state << 1) | bit) & 0x7; +} + +/* + * 'in' is 8-bits per symbol soft decision data + * 'len' is input byte length. 'out' must be + * 'len'/16 bytes long + */ + +uint8_t +ao_fec_decode(const uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void)) +{ + static uint32_t cost[2][NUM_STATE]; /* path cost */ + static bits_t bits[2][NUM_STATE]; /* save bits to quickly output them */ + + uint16_t i; /* input byte index */ + uint16_t b; /* encoded symbol index (bytes/2) */ + uint16_t o; /* output bit index */ + uint8_t p; /* previous cost/bits index */ + uint8_t n; /* next cost/bits index */ + uint8_t state; /* state index */ + const uint8_t *whiten = ao_fec_whiten_table; + uint16_t interleave; /* input byte array index */ + uint8_t s0, s1; + uint16_t avail; + uint16_t crc = AO_FEC_CRC_INIT; +#if AO_PROFILE + uint32_t start_tick; +#endif + + p = 0; + for (state = 0; state < NUM_STATE; state++) { + cost[0][state] = 0x7fffffff; + bits[0][state] = 0; + } + cost[0][0] = 0; + + if (callback) + avail = 0; + else + avail = len; + +#if AO_PROFILE + if (!avail) { + avail = callback(); + if (!avail) + return 0; + } + start_tick = ao_profile_tick(); +#endif + o = 0; + for (i = 0; i < len; i += 2) { + b = i/2; + n = p ^ 1; + + if (!avail) { + avail = callback(); + if (!avail) + return 0; + } + + /* Fetch one pair of input bytes, de-interleaving + * the input. + */ + interleave = ao_interleave_index(i); + s0 = in[interleave]; + s1 = in[interleave+1]; + + avail -= 2; + + /* Compute path costs and accumulate output bit path + * for each state and encoded bit value. Unrolling + * this loop is worth about > 30% performance boost. + * Decoding 76-byte remote access packets is reduced + * from 14.700ms to 9.3ms. Redoing the loop to + * directly compare the two pasts for each future state + * reduces this down to 5.7ms + */ + + /* Ok, of course this is tricky, it's optimized. + * + * First, it's important to realize that we have 8 + * states representing the combinations of the three + * most recent bits from the encoder. Flipping any + * of these three bits flips both output bits. + * + * 'state<<1' represents the target state for a new + * bit value of 0. '(state<<1)+1' represents the + * target state for a new bit value of 1. + * + * 'state' is the previous state with an oldest bit + * value of 0. 'state + 4' is the previous state with + * an oldest bit value of 1. These two states will + * either lead to 'state<<1' or '(state<<1)+1', depending + * on whether the next encoded bit was a zero or a one. + * + * m0 and m1 are the cost of coming to 'state<<1' from + * one of the two possible previous states 'state' and + * 'state + 4'. + * + * Because we know the expected values of each + * received bit are flipped between these two previous + * states: + * + * bitcost(state+4) = 510 - bitcost(state) + * + * With those two total costs in hand, we then pick + * the lower as the cost of the 'state<<1', and compute + * the path of bits leading to that state. + * + * Then, do the same for '(state<<1) + 1'. This time, + * instead of computing the m0 and m1 values from + * scratch, because the only difference is that we're + * expecting a one bit instead of a zero bit, we just + * flip the bitcost values around to match the + * expected transmitted bits with some tricky + * arithmetic which is equivalent to: + * + * m0 = cost[p][state] + (510 - bitcost); + * m1 = cost[p][state+4] + bitcost + * + * Then, the lowest cost and bit trace of the new state + * is saved. + */ + +#define DO_STATE(state) { \ + uint32_t bitcost; \ + \ + uint32_t m0; \ + uint32_t m1; \ + uint32_t bit; \ + \ + bitcost = ((uint32_t) (s0 ^ ao_fec_decode_table[(state<<1)]) + \ + (uint32_t) (s1 ^ ao_fec_decode_table[(state<<1)|1])); \ + \ + m0 = cost[p][state] + bitcost; \ + m1 = cost[p][state+4] + (510 - bitcost); \ + bit = m0 > m1; \ + cost[n][state<<1] = bit ? m1 : m0; \ + bits[n][state<<1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \ + \ + m0 -= (bitcost+bitcost-510); \ + m1 += (bitcost+bitcost-510); \ + bit = m0 > m1; \ + cost[n][(state<<1)+1] = bit ? m1 : m0; \ + bits[n][(state<<1)+1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \ + } + + DO_STATE(0); + DO_STATE(1); + DO_STATE(2); + DO_STATE(3); + +#if 0 + printf ("bit %3d symbol %2x %2x:", i/2, s0, s1); + for (state = 0; state < NUM_STATE; state++) { + printf (" %8u(%08x)", cost[n][state], bits[n][state]); + } + printf ("\n"); +#endif + p = n; + + /* A loop is needed to handle the last output byte. It + * won't have any bits of future data to perform full + * error correction, but we might as well give the + * best possible answer anyways. + */ + while ((b - o) >= (8 + NUM_HIST) || (i + 2 >= len && b > o)) { + + /* Compute number of bits to the end of the + * last full byte of data. This is generally + * NUM_HIST, unless we've reached + * the end of the input, in which case + * it will be seven. + */ + int8_t dist = b - (o + 8); /* distance to last ready-for-writing bit */ + uint32_t min_cost; /* lowest cost */ + uint8_t min_state; /* lowest cost state */ + uint8_t byte; + + /* Find the best fit at the current point + * of the decode. + */ + min_cost = cost[p][0]; + min_state = 0; + for (state = 1; state < NUM_STATE; state++) { + if (cost[p][state] < min_cost) { + min_cost = cost[p][state]; + min_state = state; + } + } + + /* The very last byte of data has the very last bit + * of data left in the state value; just smash the + * bits value in place and reset the 'dist' from + * -1 to 0 so that the full byte is read out + */ + if (dist < 0) { + bits[p][min_state] = (bits[p][min_state] << 1) | (min_state & 1); + dist = 0; + } + +#if 0 + printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n", + i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten); +#endif + byte = (bits[p][min_state] >> dist) ^ *whiten++; + *out++ = byte; + if (out_len > 2) + crc = ao_fec_crc_byte(byte, crc); + + if (!--out_len) { + if ((out[-2] == (uint8_t) (crc >> 8)) && + out[-1] == (uint8_t) crc) + out[-1] = AO_FEC_DECODE_CRC_OK; + else + out[-1] = 0; + out[-2] = 0; + goto done; + } + o += 8; + } + } +done: +#if AO_PROFILE + ao_fec_decode_start = start_tick; + ao_fec_decode_end = ao_profile_tick(); +#endif + return 1; +} diff --git a/src/kernel/ao_fec_tx.c b/src/kernel/ao_fec_tx.c new file mode 100644 index 00000000..4941d745 --- /dev/null +++ b/src/kernel/ao_fec_tx.c @@ -0,0 +1,134 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +#if AO_FEC_DEBUG +void +ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name) +{ + uint16_t i; + + printf ("%s (%d):", name, len); + for (i = 0; i < len; i++) { + if ((i & 7) == 0) + printf ("\n\t%02x:", i); + printf(" %02x", bytes[i]); + } + printf ("\n"); +} +#endif + +uint16_t +ao_fec_crc(const uint8_t *bytes, uint8_t len) +{ + uint16_t crc = AO_FEC_CRC_INIT; + + while (len--) + crc = ao_fec_crc_byte(*bytes++, crc); + return crc; +} + +/* + * len is the length of the data; the crc will be + * the fist two bytes after that + */ + +uint8_t +ao_fec_check_crc(const uint8_t *bytes, uint8_t len) +{ + uint16_t computed_crc = ao_fec_crc(bytes, len); + uint16_t received_crc = (bytes[len] << 8) | (bytes[len+1]); + + return computed_crc == received_crc; +} + +/* + * Compute CRC and trellis-terminator/interleave-pad bytes + */ +static uint8_t +ao_fec_prepare(const uint8_t *in, uint8_t len, uint8_t *extra) +{ + uint16_t crc = ao_fec_crc (in, len); + uint8_t i = 0; + uint8_t num_fec; + + /* Append CRC */ + extra[i++] = crc >> 8; + extra[i++] = crc; + + /* Append FEC -- 1 byte if odd, two bytes if even */ + num_fec = 2 - (i & 1); + while (num_fec--) + extra[i++] = AO_FEC_TRELLIS_TERMINATOR; + return i; +} + +const uint8_t ao_fec_whiten_table[] = { +#include "ao_whiten.h" +}; + +static const uint8_t ao_fec_encode_table[16] = { +/* next 0 1 state */ + 0, 3, /* 000 */ + 1, 2, /* 001 */ + 3, 0, /* 010 */ + 2, 1, /* 011 */ + 3, 0, /* 100 */ + 2, 1, /* 101 */ + 0, 3, /* 110 */ + 1, 2 /* 111 */ +}; + +uint8_t +ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out) +{ + uint8_t extra[AO_FEC_PREPARE_EXTRA]; + uint8_t extra_len; + uint32_t encode, interleave; + uint8_t pair, byte, bit; + uint16_t fec = 0; + const uint8_t *whiten = ao_fec_whiten_table; + + extra_len = ao_fec_prepare(in, len, extra); + for (pair = 0; pair < len + extra_len; pair += 2) { + encode = 0; + for (byte = 0; byte < 2; byte++) { + if (pair + byte == len) + in = extra; + fec |= *in++ ^ *whiten++; + for (bit = 0; bit < 8; bit++) { + encode = encode << 2 | ao_fec_encode_table[fec >> 7]; + fec = (fec << 1) & 0x7ff; + } + } + + interleave = 0; + for (bit = 0; bit < 4 * 4; bit++) { + uint8_t byte_shift = (bit & 0x3) << 3; + uint8_t bit_shift = (bit & 0xc) >> 1; + + interleave = (interleave << 2) | ((encode >> (byte_shift + bit_shift)) & 0x3); + } + *out++ = interleave >> 24; + *out++ = interleave >> 16; + *out++ = interleave >> 8; + *out++ = interleave >> 0; + } + return (len + extra_len) * 2; +} diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c new file mode 100644 index 00000000..24099347 --- /dev/null +++ b/src/kernel/ao_flight.c @@ -0,0 +1,472 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#include +#endif + +#if HAS_MPU6000 +#include +#endif + +#ifndef HAS_ACCEL +#error Please define HAS_ACCEL +#endif + +#ifndef HAS_GPS +#error Please define HAS_GPS +#endif + +#ifndef HAS_USB +#error Please define HAS_USB +#endif + +#ifndef HAS_TELEMETRY +#define HAS_TELEMETRY HAS_RADIO +#endif + +/* Main flight thread. */ + +__pdata enum ao_flight_state ao_flight_state; /* current flight state */ +__pdata uint16_t ao_boost_tick; /* time of launch detect */ +__pdata uint16_t ao_motor_number; /* number of motors burned so far */ + +#if HAS_SENSOR_ERRORS +/* Any sensor can set this to mark the flight computer as 'broken' */ +__xdata uint8_t ao_sensor_errors; +#endif + +/* + * track min/max data over a long interval to detect + * resting + */ +static __data uint16_t ao_interval_end; +static __data int16_t ao_interval_min_height; +static __data int16_t ao_interval_max_height; +#if HAS_ACCEL +static __data int16_t ao_coast_avg_accel; +#endif + +__pdata uint8_t ao_flight_force_idle; + +/* We also have a clock, which can be used to sanity check things in + * case of other failures + */ + +#define BOOST_TICKS_MAX AO_SEC_TO_TICKS(15) + +/* Landing is detected by getting constant readings from both pressure and accelerometer + * for a fairly long time (AO_INTERVAL_TICKS) + */ +#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(10) + +#define abs(a) ((a) < 0 ? -(a) : (a)) + +void +ao_flight(void) +{ + ao_sample_init(); + ao_flight_state = ao_flight_startup; + for (;;) { + + /* + * Process ADC samples, just looping + * until the sensors are calibrated. + */ + if (!ao_sample()) + continue; + + switch (ao_flight_state) { + case ao_flight_startup: + + /* Check to see what mode we should go to. + * - Invalid mode if accel cal appears to be out + * - pad mode if we're upright, + * - idle mode otherwise + */ +#if HAS_ACCEL + if (ao_config.accel_plus_g == 0 || + ao_config.accel_minus_g == 0 || + ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP || + ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP || + ao_ground_height < -1000 || + ao_ground_height > 7000) + { + /* Detected an accel value outside -1.5g to 1.5g + * (or uncalibrated values), so we go into invalid mode + */ + ao_flight_state = ao_flight_invalid; + +#if HAS_RADIO && PACKET_HAS_SLAVE + /* Turn on packet system in invalid mode on TeleMetrum */ + ao_packet_slave_start(); +#endif + } else +#endif + if (!ao_flight_force_idle +#if HAS_ACCEL + && ao_ground_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP +#endif + ) + { + /* Set pad mode - we can fly! */ + ao_flight_state = ao_flight_pad; +#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE + /* Disable the USB controller in flight mode + * to save power + */ + ao_usb_disable(); +#endif + +#if !HAS_ACCEL && PACKET_HAS_SLAVE + /* Disable packet mode in pad state on TeleMini */ + ao_packet_slave_stop(); +#endif + +#if HAS_TELEMETRY + /* Turn on telemetry system */ + ao_rdf_set(1); + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); +#endif +#if AO_LED_RED + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); +#endif + } else { + /* Set idle mode */ + ao_flight_state = ao_flight_idle; +#if HAS_SENSOR_ERRORS + if (ao_sensor_errors) + ao_flight_state = ao_flight_invalid; +#endif + +#if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE + /* Turn on packet system in idle mode on TeleMetrum */ + ao_packet_slave_start(); +#endif + +#if AO_LED_RED + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); +#endif + } + /* wakeup threads due to state change */ + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + + break; + case ao_flight_pad: + + /* pad to boost: + * + * barometer: > 20m vertical motion + * OR + * accelerometer: > 2g AND velocity > 5m/s + * + * The accelerometer should always detect motion before + * the barometer, but we use both to make sure this + * transition is detected. If the device + * doesn't have an accelerometer, then ignore the + * speed and acceleration as they are quite noisy + * on the pad. + */ + if (ao_height > AO_M_TO_HEIGHT(20) +#if HAS_ACCEL + || (ao_accel > AO_MSS_TO_ACCEL(20) && + ao_speed > AO_MS_TO_SPEED(5)) +#endif + ) + { + ao_flight_state = ao_flight_boost; + ao_boost_tick = ao_sample_tick; + + /* start logging data */ + ao_log_start(); + +#if HAS_TELEMETRY + /* Increase telemetry rate */ + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT); + + /* disable RDF beacon */ + ao_rdf_set(0); +#endif + +#if HAS_GPS + /* Record current GPS position by waking up GPS log tasks */ + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); +#endif + + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + break; + case ao_flight_boost: + + /* boost to fast: + * + * accelerometer: start to fall at > 1/4 G + * OR + * time: boost for more than 15 seconds + * + * Detects motor burn out by the switch from acceleration to + * deceleration, or by waiting until the maximum burn duration + * (15 seconds) has past. + */ + if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) || + (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) + { +#if HAS_ACCEL + ao_flight_state = ao_flight_fast; + ao_coast_avg_accel = ao_accel; +#else + ao_flight_state = ao_flight_coast; +#endif + ++ao_motor_number; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + break; +#if HAS_ACCEL + case ao_flight_fast: + /* + * This is essentially the same as coast, + * but the barometer is being ignored as + * it may be unreliable. + */ + if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) + { + ao_flight_state = ao_flight_coast; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } else + goto check_re_boost; + break; +#endif + case ao_flight_coast: + + /* + * By customer request - allow the user + * to lock out apogee detection for a specified + * number of seconds. + */ + if (ao_config.apogee_lockout) { + if ((ao_sample_tick - ao_boost_tick) < + AO_SEC_TO_TICKS(ao_config.apogee_lockout)) + break; + } + + /* apogee detect: coast to drogue deploy: + * + * speed: < 0 + * + * Also make sure the model altitude is tracking + * the measured altitude reasonably closely; otherwise + * we're probably transsonic. + */ + if (ao_speed < 0 +#if !HAS_ACCEL + && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100) +#endif + ) + { +#if HAS_IGNITE + /* ignite the drogue charge */ + ao_ignite(ao_igniter_drogue); +#endif + +#if HAS_TELEMETRY + /* slow down the telemetry system */ + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER); + + /* Turn the RDF beacon back on */ + ao_rdf_set(1); +#endif + + /* and enter drogue state */ + ao_flight_state = ao_flight_drogue; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } +#if HAS_ACCEL + else { + check_re_boost: + ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6); + if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) { + ao_boost_tick = ao_sample_tick; + ao_flight_state = ao_flight_boost; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + } +#endif + + break; + case ao_flight_drogue: + + /* drogue to main deploy: + * + * barometer: reach main deploy altitude + * + * Would like to use the accelerometer for this test, but + * the orientation of the flight computer is unknown after + * drogue deploy, so we ignore it. Could also detect + * high descent rate using the pressure sensor to + * recognize drogue deploy failure and eject the main + * at that point. Perhaps also use the drogue sense lines + * to notice continutity? + */ + if (ao_height <= ao_config.main_deploy) + { +#if HAS_IGNITE + ao_ignite(ao_igniter_main); +#endif + + /* + * Start recording min/max height + * to figure out when the rocket has landed + */ + + /* initialize interval values */ + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + + ao_interval_min_height = ao_interval_max_height = ao_avg_height; + + ao_flight_state = ao_flight_main; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + break; + + /* fall through... */ + case ao_flight_main: + + /* main to land: + * + * barometer: altitude stable + */ + + if (ao_avg_height < ao_interval_min_height) + ao_interval_min_height = ao_avg_height; + if (ao_avg_height > ao_interval_max_height) + ao_interval_max_height = ao_avg_height; + + if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { + if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4)) + { + ao_flight_state = ao_flight_landed; + + /* turn off the ADC capture */ + ao_timer_set_adc_interval(0); + + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + ao_interval_min_height = ao_interval_max_height = ao_avg_height; + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + } + break; +#if HAS_FLIGHT_DEBUG + case ao_flight_test: +#if HAS_GYRO + printf ("angle %4d pitch %7d yaw %7d roll %7d\n", + ao_sample_orient, + ((ao_sample_pitch << 9) - ao_ground_pitch) >> 9, + ((ao_sample_yaw << 9) - ao_ground_yaw) >> 9, + ((ao_sample_roll << 9) - ao_ground_roll) >> 9); +#endif + flush(); + break; +#endif /* HAS_FLIGHT_DEBUG */ + default: + break; + } + } +} + +#if HAS_FLIGHT_DEBUG +static inline int int_part(int16_t i) { return i >> 4; } +static inline int frac_part(int16_t i) { return ((i & 0xf) * 100 + 8) / 16; } + +static void +ao_flight_dump(void) +{ +#if HAS_ACCEL + int16_t accel; + + accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; +#endif + + printf ("sample:\n"); + printf (" tick %d\n", ao_sample_tick); + printf (" raw pres %d\n", ao_sample_pres); +#if HAS_ACCEL + printf (" raw accel %d\n", ao_sample_accel); +#endif + printf (" ground pres %d\n", ao_ground_pres); + printf (" ground alt %d\n", ao_ground_height); +#if HAS_ACCEL + printf (" raw accel %d\n", ao_sample_accel); + printf (" groundaccel %d\n", ao_ground_accel); + printf (" accel_2g %d\n", ao_accel_2g); +#endif + + printf (" alt %d\n", ao_sample_alt); + printf (" height %d\n", ao_sample_height); +#if HAS_ACCEL + printf (" accel %d.%02d\n", int_part(accel), frac_part(accel)); +#endif + + + printf ("kalman:\n"); + printf (" height %d\n", ao_height); + printf (" speed %d.%02d\n", int_part(ao_speed), frac_part(ao_speed)); + printf (" accel %d.%02d\n", int_part(ao_accel), frac_part(ao_accel)); + printf (" max_height %d\n", ao_max_height); + printf (" avg_height %d\n", ao_avg_height); + printf (" error_h %d\n", ao_error_h); + printf (" error_avg %d\n", ao_error_h_sq_avg); +} + +static void +ao_gyro_test(void) +{ + ao_flight_state = ao_flight_test; + ao_getchar(); + ao_flight_state = ao_flight_idle; +} + +uint8_t ao_orient_test; + +static void +ao_orient_test_select(void) +{ + ao_orient_test = !ao_orient_test; +} + +__code struct ao_cmds ao_flight_cmds[] = { + { ao_flight_dump, "F\0Dump flight status" }, + { ao_gyro_test, "G\0Test gyro code" }, + { ao_orient_test_select,"O\0Test orientation code" }, + { 0, NULL }, +}; +#endif + +static __xdata struct ao_task flight_task; + +void +ao_flight_init(void) +{ + ao_flight_state = ao_flight_startup; +#if HAS_FLIGHT_DEBUG + ao_cmd_register(&ao_flight_cmds[0]); +#endif + ao_add_task(&flight_task, ao_flight, "flight"); +} diff --git a/src/kernel/ao_flight.h b/src/kernel/ao_flight.h new file mode 100644 index 00000000..01d21c11 --- /dev/null +++ b/src/kernel/ao_flight.h @@ -0,0 +1,70 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_FLIGHT_H_ +#define _AO_FLIGHT_H_ + + +/* + * ao_flight.c + */ + +enum ao_flight_state { + ao_flight_startup = 0, + ao_flight_idle = 1, + ao_flight_pad = 2, + ao_flight_boost = 3, + ao_flight_fast = 4, + ao_flight_coast = 5, + ao_flight_drogue = 6, + ao_flight_main = 7, + ao_flight_landed = 8, + ao_flight_invalid = 9, + ao_flight_test = 10 +}; + +extern __pdata enum ao_flight_state ao_flight_state; +extern __pdata uint16_t ao_boost_tick; +extern __pdata uint16_t ao_motor_number; + +#if HAS_IMU || HAS_MMA655X +#define HAS_SENSOR_ERRORS 1 +#endif + +#if HAS_SENSOR_ERRORS +extern __xdata uint8_t ao_sensor_errors; +#endif + +extern __pdata uint16_t ao_launch_time; +extern __pdata uint8_t ao_flight_force_idle; + +/* Flight thread */ +void +ao_flight(void); + +/* Initialize flight thread */ +void +ao_flight_init(void); + +/* + * ao_flight_nano.c + */ + +void +ao_flight_nano_init(void); + +#endif /* _AO_FLIGHT_H_ */ diff --git a/src/kernel/ao_flight_nano.c b/src/kernel/ao_flight_nano.c new file mode 100644 index 00000000..406d81ad --- /dev/null +++ b/src/kernel/ao_flight_nano.c @@ -0,0 +1,120 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +/* Main flight thread. */ + +__pdata enum ao_flight_state ao_flight_state; /* current flight state */ +__pdata uint16_t ao_launch_tick; /* time of launch detect */ + +/* + * track min/max data over a long interval to detect + * resting + */ +__pdata uint16_t ao_interval_end; +__pdata alt_t ao_interval_min_height; +__pdata alt_t ao_interval_max_height; + +__pdata uint8_t ao_flight_force_idle; + +/* Landing is detected by getting constant readings from both pressure and accelerometer + * for a fairly long time (AO_INTERVAL_TICKS) + */ +#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(5) + +static void +ao_flight_nano(void) +{ + ao_sample_init(); + ao_flight_state = ao_flight_startup; + + for (;;) { + /* + * Process ADC samples, just looping + * until the sensors are calibrated. + */ + if (!ao_sample()) + continue; + + switch (ao_flight_state) { + case ao_flight_startup: + if (ao_flight_force_idle) { + /* Set idle mode */ + ao_flight_state = ao_flight_idle; + } else { + ao_flight_state = ao_flight_pad; + /* Disable packet mode in pad state */ + ao_packet_slave_stop(); + + /* Turn on telemetry system */ + ao_rdf_set(1); + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); + } + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); + + /* wakeup threads due to state change */ + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + break; + case ao_flight_pad: + if (ao_height> AO_M_TO_HEIGHT(20)) { + ao_flight_state = ao_flight_drogue; + ao_launch_tick = ao_sample_tick; + + /* start logging data */ + ao_log_start(); + + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + break; + case ao_flight_drogue: + /* drogue/main to land: + * + * barometer: altitude stable + */ + + if (ao_height < ao_interval_min_height) + ao_interval_min_height = ao_height; + if (ao_height > ao_interval_max_height) + ao_interval_max_height = ao_height; + + if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { + if (ao_interval_max_height - ao_interval_min_height < AO_M_TO_HEIGHT(5)) + { + ao_flight_state = ao_flight_landed; + + /* turn off the ADC capture */ + ao_timer_set_adc_interval(0); + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + ao_interval_min_height = ao_interval_max_height = ao_height; + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + } + break; + } + } +} + +static __xdata struct ao_task flight_task; + +void +ao_flight_nano_init(void) +{ + ao_flight_state = ao_flight_startup; + ao_add_task(&flight_task, ao_flight_nano, "flight"); +} diff --git a/src/kernel/ao_freq.c b/src/kernel/ao_freq.c new file mode 100644 index 00000000..12496f6f --- /dev/null +++ b/src/kernel/ao_freq.c @@ -0,0 +1,55 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include + +/* + * The provided 'calibration' value is + * that needed to tune the radio to precisely 434550kHz. + * Use that to 'walk' to the target frequency by following + * a 'bresenham' line from 434550kHz to the target + * frequency, and updating the radio setting along the way + */ + +int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant +{ + static __pdata int32_t set; + static __pdata uint8_t neg; + static __pdata int32_t error; + + set = 0; + neg = 0; + error = -434550 / 2; + + if ((freq -= 434550) < 0) { + neg = 1; + freq = -freq; + } + for (;;) { + if (error > 0) { + error -= 434550; + set++; + } else { + error += cal; + if (--freq < 0) + break; + } + } + if (neg) + set = -set; + return cal + set; +} diff --git a/src/kernel/ao_gps_print.c b/src/kernel/ao_gps_print.c new file mode 100644 index 00000000..47c945d7 --- /dev/null +++ b/src/kernel/ao_gps_print.c @@ -0,0 +1,112 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_GPS_TEST +#include "ao.h" +#endif +#include "ao_telem.h" + +void +ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant +{ + char state; + + if (gps_data->flags & AO_GPS_VALID) + state = AO_TELEM_GPS_STATE_LOCKED; + else if (gps_data->flags & AO_GPS_RUNNING) + state = AO_TELEM_GPS_STATE_UNLOCKED; + else + state = AO_TELEM_GPS_STATE_ERROR; + printf(AO_TELEM_GPS_STATE " %c " + AO_TELEM_GPS_NUM_SAT " %d ", + state, + (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT); + if (!(gps_data->flags & AO_GPS_VALID)) + return; + printf(AO_TELEM_GPS_LATITUDE " %ld " + AO_TELEM_GPS_LONGITUDE " %ld " + AO_TELEM_GPS_ALTITUDE " %d ", + (long) gps_data->latitude, + (long) gps_data->longitude, + gps_data->altitude); + + if (gps_data->flags & AO_GPS_DATE_VALID) + printf(AO_TELEM_GPS_YEAR " %d " + AO_TELEM_GPS_MONTH " %d " + AO_TELEM_GPS_DAY " %d ", + gps_data->year, + gps_data->month, + gps_data->day); + + printf(AO_TELEM_GPS_HOUR " %d " + AO_TELEM_GPS_MINUTE " %d " + AO_TELEM_GPS_SECOND " %d ", + gps_data->hour, + gps_data->minute, + gps_data->second); + + printf(AO_TELEM_GPS_HDOP " %d ", + gps_data->hdop * 2); + + if (gps_data->flags & AO_GPS_COURSE_VALID) { + printf(AO_TELEM_GPS_HERROR " %d " + AO_TELEM_GPS_VERROR " %d " + AO_TELEM_GPS_VERTICAL_SPEED " %d " + AO_TELEM_GPS_HORIZONTAL_SPEED " %d " + AO_TELEM_GPS_COURSE " %d ", + gps_data->h_error, + gps_data->v_error, + gps_data->climb_rate, + gps_data->ground_speed, + (int) gps_data->course * 2); + } +} + +void +ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data) __reentrant +{ + uint8_t c, n, v; + __xdata struct ao_gps_sat_orig *sat; + + n = gps_tracking_data->channels; + if (n == 0) + return; + + sat = gps_tracking_data->sats; + v = 0; + for (c = 0; c < n; c++) { + if (sat->svid) + v++; + sat++; + } + + printf (AO_TELEM_SAT_NUM " %d ", + v); + + sat = gps_tracking_data->sats; + v = 0; + for (c = 0; c < n; c++) { + if (sat->svid) { + printf (AO_TELEM_SAT_SVID "%d %d " + AO_TELEM_SAT_C_N_0 "%d %d ", + v, sat->svid, + v, sat->c_n_1); + v++; + } + sat++; + } +} diff --git a/src/kernel/ao_gps_report.c b/src/kernel/ao_gps_report.c new file mode 100644 index 00000000..07201ac2 --- /dev/null +++ b/src/kernel/ao_gps_report.c @@ -0,0 +1,89 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +void +ao_gps_report(void) +{ + static __xdata struct ao_log_record gps_log; + static __xdata struct ao_telemetry_location gps_data; + static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t date_reported = 0; + uint8_t new; + + for (;;) { + while ((new = ao_gps_new) == 0) + ao_sleep(&ao_gps_new); + ao_mutex_get(&ao_gps_mutex); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; + ao_mutex_put(&ao_gps_mutex); + + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps_time.hour = gps_data.hour; + gps_log.u.gps_time.minute = gps_data.minute; + gps_log.u.gps_time.second = gps_data.second; + gps_log.u.gps_time.flags = gps_data.flags; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_LAT; + gps_log.u.gps_latitude = gps_data.latitude; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_LON; + gps_log.u.gps_longitude = gps_data.longitude; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_ALT; + gps_log.u.gps_altitude.altitude = gps_data.altitude; + gps_log.u.gps_altitude.unused = 0xffff; + ao_log_data(&gps_log); + if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) { + gps_log.type = AO_LOG_GPS_DATE; + gps_log.u.gps_date.year = gps_data.year; + gps_log.u.gps_date.month = gps_data.month; + gps_log.u.gps_date.day = gps_data.day; + gps_log.u.gps_date.extra = 0; + date_reported = ao_log_data(&gps_log); + } + } + if (new & AO_GPS_NEW_TRACKING) { + uint8_t c, n; + + if ((n = gps_tracking_data.channels) != 0) { + gps_log.type = AO_LOG_GPS_SAT; + for (c = 0; c < n; c++) + if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid)) + { + gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1; + ao_log_data(&gps_log); + } + } + } + } +} + +__xdata struct ao_task ao_gps_report_task; + +void +ao_gps_report_init(void) +{ + ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report"); +} diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c new file mode 100644 index 00000000..5e3c71bf --- /dev/null +++ b/src/kernel/ao_gps_report_mega.c @@ -0,0 +1,90 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_log.h" + +void +ao_gps_report_mega(void) +{ + static __xdata struct ao_log_mega gps_log; + static __xdata struct ao_telemetry_location gps_data; + static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t new; + uint8_t c, n, i; + + for (;;) { + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + ao_mutex_get(&ao_gps_mutex); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; + ao_mutex_put(&ao_gps_mutex); + + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps.latitude = gps_data.latitude; + gps_log.u.gps.longitude = gps_data.longitude; + gps_log.u.gps.altitude = gps_data.altitude; + + gps_log.u.gps.hour = gps_data.hour; + gps_log.u.gps.minute = gps_data.minute; + gps_log.u.gps.second = gps_data.second; + gps_log.u.gps.flags = gps_data.flags; + gps_log.u.gps.year = gps_data.year; + gps_log.u.gps.month = gps_data.month; + gps_log.u.gps.day = gps_data.day; + gps_log.u.gps.course = gps_data.course; + gps_log.u.gps.ground_speed = gps_data.ground_speed; + gps_log.u.gps.climb_rate = gps_data.climb_rate; + gps_log.u.gps.pdop = gps_data.pdop; + gps_log.u.gps.hdop = gps_data.hdop; + gps_log.u.gps.vdop = gps_data.vdop; + gps_log.u.gps.mode = gps_data.mode; + ao_log_mega(&gps_log); + } + if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels) != 0) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_SAT; + i = 0; + for (c = 0; c < n; c++) + if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid)) + { + gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; + i++; + if (i >= 12) + break; + } + gps_log.u.gps_sat.channels = i; + ao_log_mega(&gps_log); + } + } +} + +__xdata struct ao_task ao_gps_report_mega_task; + +void +ao_gps_report_mega_init(void) +{ + ao_add_task(&ao_gps_report_mega_task, + ao_gps_report_mega, + "gps_report"); +} diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c new file mode 100644 index 00000000..696a833b --- /dev/null +++ b/src/kernel/ao_gps_report_metrum.c @@ -0,0 +1,96 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_log.h" + +void +ao_gps_report_metrum(void) +{ + static __xdata struct ao_log_metrum gps_log; + static __xdata struct ao_telemetry_location gps_data; + static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t c, n, i; + uint8_t svid; + uint8_t new; + + for (;;) { + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + ao_mutex_get(&ao_gps_mutex); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; + ao_mutex_put(&ao_gps_mutex); + + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_POS; + gps_log.u.gps.latitude = gps_data.latitude; + gps_log.u.gps.longitude = gps_data.longitude; + gps_log.u.gps.altitude = gps_data.altitude; + ao_log_metrum(&gps_log); + + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps_time.hour = gps_data.hour; + gps_log.u.gps_time.minute = gps_data.minute; + gps_log.u.gps_time.second = gps_data.second; + gps_log.u.gps_time.flags = gps_data.flags; + gps_log.u.gps_time.year = gps_data.year; + gps_log.u.gps_time.month = gps_data.month; + gps_log.u.gps_time.day = gps_data.day; + ao_log_metrum(&gps_log); + } + + if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels)) { + gps_log.type = AO_LOG_GPS_SAT; + gps_log.tick = ao_gps_tick; + i = 0; + for (c = 0; c < n; c++) { + svid = gps_tracking_data.sats[c].svid; + if (svid != 0) { + if (i == 4) { + gps_log.u.gps_sat.channels = i; + gps_log.u.gps_sat.more = 1; + ao_log_metrum(&gps_log); + i = 0; + } + gps_log.u.gps_sat.sats[i].svid = svid; + gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; + i++; + } + } + if (i) { + gps_log.u.gps_sat.channels = i; + gps_log.u.gps_sat.more = 0; + ao_log_metrum(&gps_log); + } + } + } +} + +__xdata struct ao_task ao_gps_report_metrum_task; + +void +ao_gps_report_metrum_init(void) +{ + ao_add_task(&ao_gps_report_metrum_task, + ao_gps_report_metrum, + "gps_report"); +} diff --git a/src/kernel/ao_gps_show.c b/src/kernel/ao_gps_show.c new file mode 100644 index 00000000..3a05e35a --- /dev/null +++ b/src/kernel/ao_gps_show.c @@ -0,0 +1,39 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_GPS_TEST +#include +#endif + +void +ao_gps_show(void) __reentrant +{ + uint8_t i; + ao_mutex_get(&ao_gps_mutex); + printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day); + printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); + printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude); + printf ("Alt: %d\n", ao_gps_data.altitude); + printf ("Flags: 0x%x\n", ao_gps_data.flags); + printf ("Sats: %d", ao_gps_tracking_data.channels); + for (i = 0; i < ao_gps_tracking_data.channels; i++) + printf (" %d %d", + ao_gps_tracking_data.sats[i].svid, + ao_gps_tracking_data.sats[i].c_n_1); + printf ("\ndone\n"); + ao_mutex_put(&ao_gps_mutex); +} diff --git a/src/kernel/ao_host.h b/src/kernel/ao_host.h new file mode 100644 index 00000000..6eb752c9 --- /dev/null +++ b/src/kernel/ao_host.h @@ -0,0 +1,135 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include + +#define AO_ADC_RING 64 +#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) +#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) + +/* + * One set of samples read from the A/D converter + */ +struct ao_adc { + uint16_t tick; /* tick when the sample was read */ + int16_t accel; /* accelerometer */ + int16_t pres; /* pressure sensor */ + int16_t temp; /* temperature sensor */ + int16_t v_batt; /* battery voltage */ + int16_t sense_d; /* drogue continuity sense */ + int16_t sense_m; /* main continuity sense */ +}; + +#define __pdata +#define __data +#define __xdata +#define __code +#define __reentrant + +#define DATA_TO_XDATA(a) (a) +#define PDATA_TO_XDATA(a) (a) +#define CODE_TO_XDATA(a) (a) + +enum ao_flight_state { + ao_flight_startup = 0, + ao_flight_idle = 1, + ao_flight_pad = 2, + ao_flight_boost = 3, + ao_flight_fast = 4, + ao_flight_coast = 5, + ao_flight_drogue = 6, + ao_flight_main = 7, + ao_flight_landed = 8, + ao_flight_invalid = 9 +}; + +struct ao_adc ao_adc_ring[AO_ADC_RING]; +uint8_t ao_adc_head; + +#define ao_led_on(l) +#define ao_led_off(l) +#define ao_timer_set_adc_interval(i) +#define ao_wakeup(wchan) ao_dump_state(wchan) +#define ao_cmd_register(c) +#define ao_usb_disable() +#define ao_telemetry_set_interval(x) +#define ao_delay(x) + +enum ao_igniter { + ao_igniter_drogue = 0, + ao_igniter_main = 1 +}; + +void +ao_ignite(enum ao_igniter igniter) +{ + printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); +} + +struct ao_task { + int dummy; +}; + +#define ao_add_task(t,f,n) + +#define ao_log_start() +#define ao_log_stop() + +#define AO_MS_TO_TICKS(ms) ((ms) / 10) +#define AO_SEC_TO_TICKS(s) ((s) * 100) + +#define AO_FLIGHT_TEST + +struct ao_adc ao_adc_static; + +FILE *emulator_in; + +void +ao_dump_state(void *wchan); + +void +ao_sleep(void *wchan); + +const char const * const ao_state_names[] = { + "startup", "idle", "pad", "boost", "fast", + "coast", "drogue", "main", "landed", "invalid" +}; + +struct ao_cmds { + void (*func)(void); + const char *help; +}; + + +struct ao_config { + uint16_t main_deploy; + int16_t accel_zero_g; +}; + +#define ao_config_get() + +struct ao_config ao_config = { 250, 16000 }; + +#define ao_xmemcpy(d,s,c) memcpy(d,s,c) +#define ao_xmemset(d,v,c) memset(d,v,c) +#define ao_xmemcmp(d,s,c) memcmp(d,s,c) diff --git a/src/kernel/ao_ignite.c b/src/kernel/ao_ignite.c new file mode 100644 index 00000000..823d003c --- /dev/null +++ b/src/kernel/ao_ignite.c @@ -0,0 +1,241 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include +#if AO_PYRO_NUM +#include +#endif + +#if HAS_IGNITE +__xdata struct ao_ignition ao_ignition[2]; + +void +ao_ignite(enum ao_igniter igniter) +{ + ao_arch_block_interrupts(); + ao_ignition[igniter].request = 1; + ao_wakeup(&ao_ignition); + ao_arch_release_interrupts(); +} + +#ifndef AO_SENSE_DROGUE +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_d) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) +#endif + +enum ao_igniter_status +ao_igniter_status(enum ao_igniter igniter) +{ + __xdata struct ao_data packet; + __pdata int16_t value; + __pdata uint8_t request, firing, fired; + + ao_arch_critical( + ao_data_get(&packet); + request = ao_ignition[igniter].request; + fired = ao_ignition[igniter].fired; + firing = ao_ignition[igniter].firing; + ); + if (firing || (request && !fired)) + return ao_igniter_active; + + value = (AO_IGNITER_CLOSED>>1); + switch (igniter) { + case ao_igniter_drogue: + value = AO_SENSE_DROGUE(&packet); + break; + case ao_igniter_main: + value = AO_SENSE_MAIN(&packet); + break; + } + if (value < AO_IGNITER_OPEN) + return ao_igniter_open; + else if (value > AO_IGNITER_CLOSED) + return ao_igniter_ready; + else + return ao_igniter_unknown; +} + +#ifndef AO_IGNITER_SET_DROGUE +#define AO_IGNITER_SET_DROGUE(v) AO_IGNITER_DROGUE = (v) +#define AO_IGNITER_SET_MAIN(v) AO_IGNITER_MAIN = (v) +#endif + +#ifndef AO_IGNITER_FIRE_TIME +#define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) +#endif + +#ifndef AO_IGNITER_CHARGE_TIME +#define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) +#endif + +void +ao_igniter_fire(enum ao_igniter igniter) +{ + ao_ignition[igniter].firing = 1; + switch(ao_config.ignite_mode) { + case AO_IGNITE_MODE_DUAL: + switch (igniter) { + case ao_igniter_drogue: + AO_IGNITER_SET_DROGUE(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_DROGUE(0); + break; + case ao_igniter_main: + AO_IGNITER_SET_MAIN(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_MAIN(0); + break; + } + break; + case AO_IGNITE_MODE_APOGEE: + switch (igniter) { + case ao_igniter_drogue: + AO_IGNITER_SET_DROGUE(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_DROGUE(0); + ao_delay(AO_IGNITER_CHARGE_TIME); + AO_IGNITER_SET_MAIN(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_MAIN(0); + break; + default: + break; + } + break; + case AO_IGNITE_MODE_MAIN: + switch (igniter) { + case ao_igniter_main: + AO_IGNITER_SET_DROGUE(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_DROGUE(0); + ao_delay(AO_IGNITER_CHARGE_TIME); + AO_IGNITER_SET_MAIN(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_MAIN(0); + break; + default: + break; + } + break; + } + ao_ignition[igniter].firing = 0; +} + +void +ao_igniter(void) +{ + __xdata enum ao_igniter igniter; + + ao_config_get(); + for (;;) { + ao_sleep(&ao_ignition); + for (igniter = ao_igniter_drogue; igniter <= ao_igniter_main; igniter++) { + if (ao_ignition[igniter].request && !ao_ignition[igniter].fired) { + if (igniter == ao_igniter_drogue && ao_config.apogee_delay) + ao_delay(AO_SEC_TO_TICKS(ao_config.apogee_delay)); + + ao_igniter_fire(igniter); + ao_delay(AO_IGNITER_CHARGE_TIME); + ao_ignition[igniter].fired = 1; + } + } + } +} + +#endif + +void +ao_ignite_manual(void) +{ + ao_cmd_white(); + if (!ao_match_word("DoIt")) + return; + ao_cmd_white(); +#if HAS_IGNITE + if (ao_cmd_lex_c == 'm' && ao_match_word("main")) { + ao_igniter_fire(ao_igniter_main); + return; + } + if (ao_cmd_lex_c == 'd' && ao_match_word("drogue")) { + ao_igniter_fire(ao_igniter_drogue); + return; + } +#endif +#if AO_PYRO_NUM + if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') { + ao_pyro_manual(ao_cmd_lex_c - '0'); + return; + } +#endif + ao_cmd_status = ao_cmd_syntax_error; +} + +__code char * __code ao_igniter_status_names[] = { + "unknown", "ready", "active", "open" +}; + +#if HAS_IGNITE +void +ao_ignite_print_status(enum ao_igniter igniter, __code char *name) __reentrant +{ + enum ao_igniter_status status = ao_igniter_status(igniter); + printf("Igniter: %6s Status: %s\n", + name, + ao_igniter_status_names[status]); +} +#endif + +void +ao_ignite_test(void) +{ +#if HAS_IGNITE + ao_ignite_print_status(ao_igniter_drogue, "drogue"); + ao_ignite_print_status(ao_igniter_main, "main"); +#endif +#if AO_PYRO_NUM + ao_pyro_print_status(); +#endif +} + +__code struct ao_cmds ao_ignite_cmds[] = { + { ao_ignite_manual, "i {main|drogue}\0Fire igniter. is doit with D&I" }, + { ao_ignite_test, "t\0Test igniter" }, + { 0, NULL }, +}; + +#if HAS_IGNITE +__xdata struct ao_task ao_igniter_task; + +void +ao_ignite_set_pins(void) +{ + ao_enable_output(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, AO_IGNITER_DROGUE, 0); + ao_enable_output(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, 0); +} +#endif + +void +ao_igniter_init(void) +{ +#if HAS_IGNITE + ao_ignite_set_pins(); + ao_add_task(&ao_igniter_task, ao_igniter, "igniter"); +#endif + ao_cmd_register(&ao_ignite_cmds[0]); +} diff --git a/src/kernel/ao_int64.c b/src/kernel/ao_int64.c new file mode 100644 index 00000000..aa23dbe0 --- /dev/null +++ b/src/kernel/ao_int64.c @@ -0,0 +1,158 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include + +__pdata ao_int64_t *__data ao_64r, *__data ao_64a, *__data ao_64b; + +void ao_plus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR { + __LOCAL uint32_t t; + + r->high = a->high + b->high; + t = a->low + b->low; + if (t < a->low) + r->high++; + r->low = t; +} + +void ao_minus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR { + __LOCAL uint32_t t; + + r->high = a->high - b->high; + t = a->low - b->low; + if (t > a->low) + r->high--; + r->low = t; +} + +void ao_rshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR { + if (d < 32) { + r->low = a->low >> d; + if (d) + r->low |= a->high << (32 - d); + r->high = (int32_t) a->high >> d; + } else { + d &= 0x1f; + r->low = (int32_t) a->high >> d; + r->high = 0; + } +} + +void ao_lshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR { + if (d < 32) { + r->high = a->high << d; + if (d) + r->high |= a->low >> (32 - d); + r->low = a->low << d; + } else { + d &= 0x1f; + r->high = a->low << d; + r->low = 0; + } +} + +static void ao_umul64_32_32(__ARG ao_int64_t *r, uint32_t a, uint32_t b) __reentrant { + __LOCAL uint32_t s; + __LOCAL ao_int64_t t; + r->low = (uint32_t) (uint16_t) a * (uint16_t) b; + r->high = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16); + + s = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b; + + t.high = s >> 16; + t.low = s << 16; + ao_plus64(r, r, &t); + + s = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16); + + t.high = s >> 16; + t.low = s << 16; + ao_plus64(r, r, &t); +} + +void ao_neg64(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR { + r->high = ~a->high; + if (!(r->low = ~a->low + 1)) + r->high++; +} + +void ao_mul64_32_32(__ARG ao_int64_t *r, int32_t a, int32_t b) __FATTR { + uint8_t negative = 0; + + if (a < 0) { + a = -a; + ++negative; + } + if (b < 0) { + b = -b; + --negative; + } + ao_umul64_32_32(r, a, b); + if (negative) + ao_neg64(r, r); +} + +static void ao_umul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __reentrant { + __LOCAL ao_int64_t r2, r3; + + ao_umul64_32_32(&r2, a->high, b->low); + ao_umul64_32_32(&r3, a->low, b->high); + ao_umul64_32_32(r, a->low, b->low); + + r->high += r2.low + r3.low; +} + +static __ARG ao_int64_t ap, bp; + +void ao_mul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __FATTR { + uint8_t negative = 0; + + if (ao_int64_negativep(a)) { + ao_neg64(&ap, a); + a = ≈ + ++negative; + } + if (ao_int64_negativep(b)) { + ao_neg64(&bp, b); + b = &bp; + --negative; + } + ao_umul64(r, a, b); + if (negative) + ao_neg64(r, r); +} + +static void ao_umul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, uint16_t b) __reentrant { + __LOCAL uint32_t h; + + h = a->high * b; + ao_umul64_32_32(r, a->low, b); + r->high += h; +} + +void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR { + uint8_t negative = 0; + + if ((int32_t) a->high < 0) { + ao_neg64(&ap, a); + a = ≈ + negative++; + } else + ao_umul64_64_16(r, a, b); + if (negative) + ao_neg64(r, r); +} diff --git a/src/kernel/ao_int64.h b/src/kernel/ao_int64.h new file mode 100644 index 00000000..b16db58c --- /dev/null +++ b/src/kernel/ao_int64.h @@ -0,0 +1,48 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_INT64_H_ +#define _AO_INT64_H_ + +#include + +typedef struct { + uint32_t high; + uint32_t low; +} ao_int64_t; + +#define __FATTR +#define __ARG __pdata +#define __LOCAL static __pdata + +void ao_plus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR; +void ao_minus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR; +void ao_neg64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a) __FATTR; +void ao_rshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR; +void ao_lshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR; +void ao_mul64_32_32(__ARG ao_int64_t *r, __ARG int32_t a, __ARG int32_t b) __FATTR; +void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR; +void ao_mul64(__ARG ao_int64_t * __ARG r, __ARG ao_int64_t * __ARG a, __ARG ao_int64_t *__ARG b) __FATTR; + +#define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a)) +#define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b)) + +#define ao_cast64(a) (((int64_t) (a)->high << 32) | (a)->low) + +#define ao_int64_negativep(a) (((int32_t) (a)->high) < 0) + +#endif /* _AO_INT64_H_ */ diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c new file mode 100644 index 00000000..9aea1f14 --- /dev/null +++ b/src/kernel/ao_kalman.c @@ -0,0 +1,295 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#include "ao_flight.h" +#endif + +#include "ao_sample.h" +#include "ao_kalman.h" + + +static __pdata int32_t ao_k_height; +static __pdata int32_t ao_k_speed; +static __pdata int32_t ao_k_accel; + +#define AO_K_STEP_100 to_fix16(0.01) +#define AO_K_STEP_2_2_100 to_fix16(0.00005) + +#define AO_K_STEP_10 to_fix16(0.1) +#define AO_K_STEP_2_2_10 to_fix16(0.005) + +#define AO_K_STEP_1 to_fix16(1) +#define AO_K_STEP_2_2_1 to_fix16(0.5) + +__pdata int16_t ao_height; +__pdata int16_t ao_speed; +__pdata int16_t ao_accel; +__xdata int16_t ao_max_height; +static __pdata int32_t ao_avg_height_scaled; +__xdata int16_t ao_avg_height; + +__pdata int16_t ao_error_h; +__pdata int16_t ao_error_h_sq_avg; + +#if HAS_ACCEL +__pdata int16_t ao_error_a; +#endif + +static void +ao_kalman_predict(void) +{ +#ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) { + ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 + + (int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4; + ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1; + + return; + } + if (ao_sample_tick - ao_sample_prev_tick > 5) { + ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 + + (int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4; + ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10; + + return; + } + if (ao_flight_debug) { + printf ("predict speed %g + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0, + (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); + } +#endif + ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 + + (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4; + ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100; +} + +static void +ao_kalman_err_height(void) +{ + int16_t e; + int16_t height_distrust; +#if HAS_ACCEL + int16_t speed_distrust; +#endif + + ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16); + + e = ao_error_h; + if (e < 0) + e = -e; + if (e > 127) + e = 127; +#if HAS_ACCEL + ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2; + ao_error_h_sq_avg += (e * e) >> 2; +#else + ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4; + ao_error_h_sq_avg += (e * e) >> 4; +#endif + + if (ao_flight_state >= ao_flight_drogue) + return; + height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT; +#if HAS_ACCEL + /* speed is stored * 16, but we need to ramp between 200 and 328, so + * we want to multiply by 2. The result is a shift by 3. + */ + speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1); + if (speed_distrust <= 0) + speed_distrust = 0; + else if (speed_distrust > height_distrust) + height_distrust = speed_distrust; +#endif + if (height_distrust > 0) { +#ifdef AO_FLIGHT_TEST + int old_ao_error_h = ao_error_h; +#endif + if (height_distrust > 0x100) + height_distrust = 0x100; + ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8); +#ifdef AO_FLIGHT_TEST + if (ao_flight_debug) { + printf("over height %g over speed %g distrust: %g height: error %d -> %d\n", + (double) (ao_sample_alt - AO_MAX_BARO_HEIGHT), + (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) / 16.0, + height_distrust / 256.0, + old_ao_error_h, ao_error_h); + } +#endif + } +} + +static void +ao_kalman_correct_baro(void) +{ + ao_kalman_err_height(); +#ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) { + ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h; + ao_k_speed += (int32_t) AO_BARO_K1_1 * ao_error_h; + ao_k_accel += (int32_t) AO_BARO_K2_1 * ao_error_h; + return; + } + if (ao_sample_tick - ao_sample_prev_tick > 5) { + ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h; + ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h; + ao_k_accel += (int32_t) AO_BARO_K2_10 * ao_error_h; + return; + } +#endif + ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h; + ao_k_speed += (int32_t) AO_BARO_K1_100 * ao_error_h; + ao_k_accel += (int32_t) AO_BARO_K2_100 * ao_error_h; +} + +#if HAS_ACCEL + +static void +ao_kalman_err_accel(void) +{ + int32_t accel; + + accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; + + /* Can't use ao_accel here as it is the pre-prediction value still */ + ao_error_a = (accel - ao_k_accel) >> 16; +} + +#ifndef FORCE_ACCEL +static void +ao_kalman_correct_both(void) +{ + ao_kalman_err_height(); + ao_kalman_err_accel(); + +#ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) { + if (ao_flight_debug) { + printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), + (double) ao_error_h, AO_BOTH_K10_1 / 65536.0, + (double) ao_error_a, AO_BOTH_K11_1 / 65536.0, + (ao_k_speed + + (int32_t) AO_BOTH_K10_1 * ao_error_h + + (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0)); + } + ao_k_height += + (int32_t) AO_BOTH_K00_1 * ao_error_h + + (int32_t) AO_BOTH_K01_1 * ao_error_a; + ao_k_speed += + (int32_t) AO_BOTH_K10_1 * ao_error_h + + (int32_t) AO_BOTH_K11_1 * ao_error_a; + ao_k_accel += + (int32_t) AO_BOTH_K20_1 * ao_error_h + + (int32_t) AO_BOTH_K21_1 * ao_error_a; + return; + } + if (ao_sample_tick - ao_sample_prev_tick > 5) { + if (ao_flight_debug) { + printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), + (double) ao_error_h, AO_BOTH_K10_10 / 65536.0, + (double) ao_error_a, AO_BOTH_K11_10 / 65536.0, + (ao_k_speed + + (int32_t) AO_BOTH_K10_10 * ao_error_h + + (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); + } + ao_k_height += + (int32_t) AO_BOTH_K00_10 * ao_error_h + + (int32_t) AO_BOTH_K01_10 * ao_error_a; + ao_k_speed += + (int32_t) AO_BOTH_K10_10 * ao_error_h + + (int32_t) AO_BOTH_K11_10 * ao_error_a; + ao_k_accel += + (int32_t) AO_BOTH_K20_10 * ao_error_h + + (int32_t) AO_BOTH_K21_10 * ao_error_a; + return; + } + if (ao_flight_debug) { + printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), + (double) ao_error_h, AO_BOTH_K10_100 / 65536.0, + (double) ao_error_a, AO_BOTH_K11_100 / 65536.0, + (ao_k_speed + + (int32_t) AO_BOTH_K10_100 * ao_error_h + + (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); + } +#endif + ao_k_height += + (int32_t) AO_BOTH_K00_100 * ao_error_h + + (int32_t) AO_BOTH_K01_100 * ao_error_a; + ao_k_speed += + (int32_t) AO_BOTH_K10_100 * ao_error_h + + (int32_t) AO_BOTH_K11_100 * ao_error_a; + ao_k_accel += + (int32_t) AO_BOTH_K20_100 * ao_error_h + + (int32_t) AO_BOTH_K21_100 * ao_error_a; +} + +#else + +static void +ao_kalman_correct_accel(void) +{ + ao_kalman_err_accel(); + + if (ao_sample_tick - ao_sample_prev_tick > 5) { + ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a; + ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a; + ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a; + return; + } + ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a; + ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a; + ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a; +} + +#endif /* else FORCE_ACCEL */ +#endif /* HAS_ACCEL */ + +void +ao_kalman(void) +{ + ao_kalman_predict(); +#if HAS_ACCEL + if (ao_flight_state <= ao_flight_coast) { +#ifdef FORCE_ACCEL + ao_kalman_correct_accel(); +#else + ao_kalman_correct_both(); +#endif + } else +#endif + ao_kalman_correct_baro(); + ao_height = from_fix(ao_k_height); + ao_speed = from_fix(ao_k_speed); + ao_accel = from_fix(ao_k_accel); + if (ao_height > ao_max_height) + ao_max_height = ao_height; + ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height; +#ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) + ao_avg_height = (ao_avg_height_scaled + 1) >> 1; + else if (ao_sample_tick - ao_sample_prev_tick > 5) + ao_avg_height = (ao_avg_height_scaled + 7) >> 4; + else +#endif + ao_avg_height = (ao_avg_height_scaled + 63) >> 7; +} diff --git a/src/kernel/ao_lcd.h b/src/kernel/ao_lcd.h new file mode 100644 index 00000000..f7e1391a --- /dev/null +++ b/src/kernel/ao_lcd.h @@ -0,0 +1,60 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_LCD_H_ +#define _AO_LCD_H_ + +/* ao_lcd.c */ + +void +ao_lcd_putchar(uint8_t d); + +void +ao_lcd_putstring(char *string); + +void +ao_lcd_contrast_set(uint8_t contrast); + +void +ao_lcd_clear(void); + +void +ao_lcd_cursor_on(void); + +void +ao_lcd_cursor_off(void); + +#define AO_LCD_ADDR(row,col) ((row << 6) | (col)) + +void +ao_lcd_goto(uint8_t addr); + +void +ao_lcd_start(void); + +void +ao_lcd_init(void); + +/* ao_lcd_port.c */ + +void +ao_lcd_port_put_nibble(uint8_t rs, uint8_t d); + +void +ao_lcd_port_init(void); + +#endif /* _AO_LCD_H_ */ diff --git a/src/kernel/ao_led.h b/src/kernel/ao_led.h new file mode 100644 index 00000000..d9a0914a --- /dev/null +++ b/src/kernel/ao_led.h @@ -0,0 +1,59 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_LED_H_ +#define _AO_LED_H_ + +/* + * ao_led.c + */ + +#define AO_LED_NONE 0 + +#ifndef AO_LED_TYPE +#define AO_LED_TYPE uint8_t +#endif + +/* Turn on the specified LEDs */ +void +ao_led_on(AO_LED_TYPE colors); + +/* Turn off the specified LEDs */ +void +ao_led_off(AO_LED_TYPE colors); + +/* Set all of the LEDs to the specified state */ +void +ao_led_set(AO_LED_TYPE colors); + +/* Set all LEDs in 'mask' to the specified state */ +void +ao_led_set_mask(uint8_t colors, uint8_t mask); + +/* Toggle the specified LEDs */ +void +ao_led_toggle(AO_LED_TYPE colors); + +/* Turn on the specified LEDs for the indicated interval */ +void +ao_led_for(AO_LED_TYPE colors, uint16_t ticks) __reentrant; + +/* Initialize the LEDs */ +void +ao_led_init(AO_LED_TYPE enable); + +#endif /* _AO_LED_H_ */ diff --git a/src/kernel/ao_list.h b/src/kernel/ao_list.h new file mode 100644 index 00000000..8a6fa4d9 --- /dev/null +++ b/src/kernel/ao_list.h @@ -0,0 +1,213 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_LIST_H_ +#define _AO_LIST_H_ + +#include + +struct ao_list { + struct ao_list *next, *prev; +}; + +static inline void +ao_list_init(struct ao_list *list) +{ + list->next = list->prev = list; +} + +static inline void +__ao_list_add(struct ao_list *list, struct ao_list *prev, struct ao_list *next) +{ + next->prev = list; + list->next = next; + list->prev = prev; + prev->next = list; +} + +/** + * Insert a new element after the given list head. The new element does not + * need to be initialised as empty list. + * The list changes from: + * head → some element → ... + * to + * head → new element → older element → ... + * + * Example: + * struct foo *newfoo = malloc(...); + * ao_list_add(&newfoo->entry, &bar->list_of_foos); + * + * @param entry The new element to prepend to the list. + * @param head The existing list. + */ +static inline void +ao_list_insert(struct ao_list *entry, struct ao_list *head) +{ + __ao_list_add(entry, head, head->next); +} + +/** + * Append a new element to the end of the list given with this list head. + * + * The list changes from: + * head → some element → ... → lastelement + * to + * head → some element → ... → lastelement → new element + * + * Example: + * struct foo *newfoo = malloc(...); + * ao_list_append(&newfoo->entry, &bar->list_of_foos); + * + * @param entry The new element to prepend to the list. + * @param head The existing list. + */ +static inline void +ao_list_append(struct ao_list *entry, struct ao_list *head) +{ + __ao_list_add(entry, head->prev, head); +} + +static inline void +__ao_list_del(struct ao_list *prev, struct ao_list *next) +{ + next->prev = prev; + prev->next = next; +} + +/** + * Remove the element from the list it is in. Using this function will reset + * the pointers to/from this element so it is removed from the list. It does + * NOT free the element itself or manipulate it otherwise. + * + * Using ao_list_del on a pure list head (like in the example at the top of + * this file) will NOT remove the first element from + * the list but rather reset the list as empty list. + * + * Example: + * ao_list_del(&foo->entry); + * + * @param entry The element to remove. + */ +static inline void +ao_list_del(struct ao_list *entry) +{ + __ao_list_del(entry->prev, entry->next); + ao_list_init(entry); +} + +/** + * Check if the list is empty. + * + * Example: + * ao_list_is_empty(&bar->list_of_foos); + * + * @return True if the list contains one or more elements or False otherwise. + */ +static inline uint8_t +ao_list_is_empty(struct ao_list *head) +{ + return head->next == head; +} + +/** + * Returns a pointer to the container of this list element. + * + * Example: + * struct foo* f; + * f = container_of(&foo->entry, struct foo, entry); + * assert(f == foo); + * + * @param ptr Pointer to the struct ao_list. + * @param type Data type of the list element. + * @param member Member name of the struct ao_list field in the list element. + * @return A pointer to the data struct containing the list head. + */ +#define ao_container_of(ptr, type, member) \ + ((type *)((char *)(ptr) - offsetof(type, member))) + +/** + * Alias of ao_container_of + */ +#define ao_list_entry(ptr, type, member) \ + ao_container_of(ptr, type, member) + +/** + * Retrieve the first list entry for the given list pointer. + * + * Example: + * struct foo *first; + * first = ao_list_first_entry(&bar->list_of_foos, struct foo, list_of_foos); + * + * @param ptr The list head + * @param type Data type of the list element to retrieve + * @param member Member name of the struct ao_list field in the list element. + * @return A pointer to the first list element. + */ +#define ao_list_first_entry(ptr, type, member) \ + ao_list_entry((ptr)->next, type, member) + +/** + * Retrieve the last list entry for the given listpointer. + * + * Example: + * struct foo *first; + * first = ao_list_last_entry(&bar->list_of_foos, struct foo, list_of_foos); + * + * @param ptr The list head + * @param type Data type of the list element to retrieve + * @param member Member name of the struct ao_list field in the list element. + * @return A pointer to the last list element. + */ +#define ao_list_last_entry(ptr, type, member) \ + ao_list_entry((ptr)->prev, type, member) + +/** + * Loop through the list given by head and set pos to struct in the list. + * + * Example: + * struct foo *iterator; + * ao_list_for_each_entry(iterator, &bar->list_of_foos, entry) { + * [modify iterator] + * } + * + * This macro is not safe for node deletion. Use ao_list_for_each_entry_safe + * instead. + * + * @param pos Iterator variable of the type of the list elements. + * @param head List head + * @param member Member name of the struct ao_list in the list elements. + * + */ +#define ao_list_for_each_entry(pos, head, type, member) \ + for (pos = ao_container_of((head)->next, type, member); \ + &pos->member != (head); \ + pos = ao_container_of(pos->member.next, type, member)) + +/** + * Loop through the list, keeping a backup pointer to the element. This + * macro allows for the deletion of a list element while looping through the + * list. + * + * See ao_list_for_each_entry for more details. + */ +#define ao_list_for_each_entry_safe(pos, tmp, head, type, member) \ + for ((pos = ao_container_of((head)->next, type, member)), \ + (tmp = ao_container_of(pos->member.next, type, member)); \ + &pos->member != (head); \ + (pos = tmp), (tmp = ao_container_of(pos->member.next, type, member))) + +#endif /* _AO_LIST_H_ */ diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c new file mode 100644 index 00000000..20febefe --- /dev/null +++ b/src/kernel/ao_log.c @@ -0,0 +1,291 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include +#include + +__pdata uint32_t ao_log_current_pos; +__pdata uint32_t ao_log_end_pos; +__pdata uint32_t ao_log_start_pos; +__xdata uint8_t ao_log_running; +__pdata enum ao_flight_state ao_log_state; +__xdata uint16_t ao_flight_number; + +void +ao_log_flush(void) +{ + ao_storage_flush(); +} + +/* + * When erasing a flight log, make sure the config block + * has an up-to-date version of the current flight number + */ + +struct ao_log_erase { + uint8_t unused; + uint16_t flight; +}; + +static __xdata struct ao_log_erase erase; + +#define LOG_MAX_ERASE 16 + +static uint32_t +ao_log_erase_pos(uint8_t i) +{ + return i * sizeof (struct ao_log_erase) + AO_CONFIG_MAX_SIZE; +} + +void +ao_log_write_erase(uint8_t pos) +{ + erase.unused = 0x00; + erase.flight = ao_flight_number; + ao_config_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); + ao_config_flush(); +} + +static void +ao_log_read_erase(uint8_t pos) +{ + ao_config_read(ao_log_erase_pos(pos), &erase, sizeof (erase)); +} + + +static void +ao_log_erase_mark(void) +{ + uint8_t i; + + for (i = 0; i < LOG_MAX_ERASE; i++) { + ao_log_read_erase(i); + if (erase.unused == 0 && erase.flight == ao_flight_number) + return; + if (erase.unused == 0xff) { + ao_log_write_erase(i); + return; + } + } + ao_config_put(); +} + +static uint8_t +ao_log_slots() +{ + return (uint8_t) (ao_storage_log_max / ao_config.flight_log_max); +} + +uint32_t +ao_log_pos(uint8_t slot) +{ + return ((slot) * ao_config.flight_log_max); +} + +static uint16_t +ao_log_max_flight(void) +{ + uint8_t log_slot; + uint8_t log_slots; + uint16_t log_flight; + uint16_t max_flight = 0; + + /* Scan the log space looking for the biggest flight number */ + log_slots = ao_log_slots(); + for (log_slot = 0; log_slot < log_slots; log_slot++) { + log_flight = ao_log_flight(log_slot); + if (!log_flight) + continue; + if (max_flight == 0 || (int16_t) (log_flight - max_flight) > 0) + max_flight = log_flight; + } + return max_flight; +} + +void +ao_log_scan(void) __reentrant +{ + uint8_t log_slot; + uint8_t log_slots; + uint8_t log_want; + + ao_config_get(); + + ao_flight_number = ao_log_max_flight(); + if (ao_flight_number) + if (++ao_flight_number == 0) + ao_flight_number = 1; + + /* Now look through the log of flight numbers from erase operations and + * see if the last one is bigger than what we found above + */ + for (log_slot = LOG_MAX_ERASE; log_slot-- > 0;) { + ao_log_read_erase(log_slot); + if (erase.unused == 0) { + if (ao_flight_number == 0 || + (int16_t) (erase.flight - ao_flight_number) > 0) + ao_flight_number = erase.flight; + break; + } + } + if (ao_flight_number == 0) + ao_flight_number = 1; + + /* With a flight number in hand, find a place to write a new log, + * use the target flight number to index the available log slots so + * that we write logs to each spot about the same number of times. + */ + + /* Find a log slot for the next flight, if available */ + ao_log_current_pos = ao_log_end_pos = 0; + log_slots = ao_log_slots(); + log_want = (ao_flight_number - 1) % log_slots; + log_slot = log_want; + do { + if (ao_log_flight(log_slot) == 0) { + ao_log_current_pos = ao_log_pos(log_slot); + ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; + break; + } + if (++log_slot >= log_slots) + log_slot = 0; + } while (log_slot != log_want); + + ao_wakeup(&ao_flight_number); +} + +void +ao_log_start(void) +{ + /* start logging */ + ao_log_running = 1; + ao_wakeup(&ao_log_running); +} + +void +ao_log_stop(void) +{ + ao_log_running = 0; + ao_log_flush(); +} + +uint8_t +ao_log_present(void) +{ + return ao_log_max_flight() != 0; +} + +uint8_t +ao_log_full(void) +{ + return ao_log_current_pos == ao_log_end_pos; +} + +#if HAS_ADC +static __xdata struct ao_task ao_log_task; +#endif + +void +ao_log_list(void) __reentrant +{ + uint8_t slot; + uint8_t slots; + uint16_t flight; + + slots = ao_log_slots(); + for (slot = 0; slot < slots; slot++) + { + flight = ao_log_flight(slot); + if (flight) + printf ("flight %d start %x end %x\n", + flight, + (uint16_t) (ao_log_pos(slot) >> 8), + (uint16_t) (ao_log_pos(slot+1) >> 8)); + } + printf ("done\n"); +} + +void +ao_log_delete(void) __reentrant +{ + uint8_t slot; + uint8_t slots; + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + + slots = ao_log_slots(); + /* Look for the flight log matching the requested flight */ + if (ao_cmd_lex_i) { + for (slot = 0; slot < slots; slot++) { + if (ao_log_flight(slot) == ao_cmd_lex_i) { + ao_log_erase_mark(); + ao_log_current_pos = ao_log_pos(slot); + ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; + while (ao_log_current_pos < ao_log_end_pos) { + uint8_t i; + static __xdata uint8_t b; + + /* + * Check to see if we've reached the end of + * the used memory to avoid re-erasing the same + * memory over and over again + */ + for (i = 0; i < 16; i++) { + if (ao_storage_read(ao_log_current_pos + i, &b, 1)) + if (b != 0xff) + break; + } + if (i == 16) + break; + ao_storage_erase(ao_log_current_pos); + ao_log_current_pos += ao_storage_block; + } + puts("Erased"); + return; + } + } + } + printf("No such flight: %d\n", ao_cmd_lex_i); +} + +__code struct ao_cmds ao_log_cmds[] = { + { ao_log_list, "l\0List logs" }, + { ao_log_delete, "d \0Delete flight" }, + { 0, NULL }, +}; + +void +ao_log_init(void) +{ + ao_log_running = 0; + + /* For now, just log the flight starting at the begining of eeprom */ + ao_log_state = ao_flight_invalid; + + ao_cmd_register(&ao_log_cmds[0]); + +#ifndef HAS_ADC +#error Define HAS_ADC for ao_log.c +#endif +#if HAS_ADC + /* Create a task to log events to eeprom */ + ao_add_task(&ao_log_task, ao_log, "log"); +#endif +} diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h new file mode 100644 index 00000000..09f31188 --- /dev/null +++ b/src/kernel/ao_log.h @@ -0,0 +1,386 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_LOG_H_ +#define _AO_LOG_H_ + +#include + +/* + * ao_log.c + */ + +/* We record flight numbers in the first record of + * the log. Tasks may wait for this to be initialized + * by sleeping on this variable. + */ +extern __xdata uint16_t ao_flight_number; + +extern __pdata uint32_t ao_log_current_pos; +extern __pdata uint32_t ao_log_end_pos; +extern __pdata uint32_t ao_log_start_pos; +extern __xdata uint8_t ao_log_running; +extern __pdata enum ao_flight_state ao_log_state; + +/* required functions from the underlying log system */ + +#define AO_LOG_FORMAT_UNKNOWN 0 /* unknown; altosui will have to guess */ +#define AO_LOG_FORMAT_FULL 1 /* 8 byte typed log records */ +#define AO_LOG_FORMAT_TINY 2 /* two byte state/baro records */ +#define AO_LOG_FORMAT_TELEMETRY 3 /* 32 byte ao_telemetry records */ +#define AO_LOG_FORMAT_TELESCIENCE 4 /* 32 byte typed telescience records */ +#define AO_LOG_FORMAT_TELEMEGA 5 /* 32 byte typed telemega records */ +#define AO_LOG_FORMAT_EASYMINI 6 /* 16-byte MS5607 baro only, 3.0V supply */ +#define AO_LOG_FORMAT_TELEMETRUM 7 /* 16-byte typed telemetrum records */ +#define AO_LOG_FORMAT_TELEMINI 8 /* 16-byte MS5607 baro only, 3.3V supply */ +#define AO_LOG_FORMAT_NONE 127 /* No log at all */ + +extern __code uint8_t ao_log_format; + +/* Return the flight number from the given log slot, 0 if none */ +uint16_t +ao_log_flight(uint8_t slot); + +/* Flush the log */ +void +ao_log_flush(void); + +/* Logging thread main routine */ +void +ao_log(void); + +/* functions provided in ao_log.c */ + +/* Figure out the current flight number */ +void +ao_log_scan(void) __reentrant; + +/* Return the position of the start of the given log slot */ +uint32_t +ao_log_pos(uint8_t slot); + +/* Start logging to eeprom */ +void +ao_log_start(void); + +/* Stop logging */ +void +ao_log_stop(void); + +/* Initialize the logging system */ +void +ao_log_init(void); + +/* Write out the current flight number to the erase log */ +void +ao_log_write_erase(uint8_t pos); + +/* Returns true if there are any logs stored in eeprom */ +uint8_t +ao_log_present(void); + +/* Returns true if there is no more storage space available */ +uint8_t +ao_log_full(void); + +/* + * ao_log_big.c + */ + +/* + * The data log is recorded in the eeprom as a sequence + * of data packets. + * + * Each packet starts with a 4-byte header that has the + * packet type, the packet checksum and the tick count. Then + * they all contain 2 16 bit values which hold packet-specific + * data. + * + * For each flight, the first packet + * is FLIGHT packet, indicating the serial number of the + * device and a unique number marking the number of flights + * recorded by this device. + * + * During flight, data from the accelerometer and barometer + * are recorded in SENSOR packets, using the raw 16-bit values + * read from the A/D converter. + * + * Also during flight, but at a lower rate, the deployment + * sensors are recorded in DEPLOY packets. The goal here is to + * detect failure in the deployment circuits. + * + * STATE packets hold state transitions as the flight computer + * transitions through different stages of the flight. + */ +#define AO_LOG_FLIGHT 'F' +#define AO_LOG_SENSOR 'A' +#define AO_LOG_TEMP_VOLT 'T' +#define AO_LOG_DEPLOY 'D' +#define AO_LOG_STATE 'S' +#define AO_LOG_GPS_TIME 'G' +#define AO_LOG_GPS_LAT 'N' +#define AO_LOG_GPS_LON 'W' +#define AO_LOG_GPS_ALT 'H' +#define AO_LOG_GPS_SAT 'V' +#define AO_LOG_GPS_DATE 'Y' +#define AO_LOG_GPS_POS 'P' + +#define AO_LOG_POS_NONE (~0UL) + +struct ao_log_record { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { + struct { + int16_t ground_accel; /* 4 */ + uint16_t flight; /* 6 */ + } flight; + struct { + int16_t accel; /* 4 */ + int16_t pres; /* 6 */ + } sensor; + struct { + int16_t temp; + int16_t v_batt; + } temp_volt; + struct { + int16_t drogue; + int16_t main; + } deploy; + struct { + uint16_t state; + uint16_t reason; + } state; + struct { + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t flags; + } gps_time; + int32_t gps_latitude; + int32_t gps_longitude; + struct { + int16_t altitude; + uint16_t unused; + } gps_altitude; + struct { + uint16_t svid; + uint8_t unused; + uint8_t c_n; + } gps_sat; + struct { + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t extra; + } gps_date; + struct { + uint16_t d0; + uint16_t d1; + } anon; + } u; +}; + +struct ao_log_mega { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + int16_t ground_accel_along; /* 16 */ + int16_t ground_accel_across; /* 12 */ + int16_t ground_accel_through; /* 14 */ + int16_t ground_roll; /* 18 */ + int16_t ground_pitch; /* 20 */ + int16_t ground_yaw; /* 22 */ + } flight; /* 24 */ + /* AO_LOG_STATE */ + struct { + uint16_t state; + uint16_t reason; + } state; + /* AO_LOG_SENSOR */ + struct { + uint32_t pres; /* 4 */ + uint32_t temp; /* 8 */ + int16_t accel_x; /* 12 */ + int16_t accel_y; /* 14 */ + int16_t accel_z; /* 16 */ + int16_t gyro_x; /* 18 */ + int16_t gyro_y; /* 20 */ + int16_t gyro_z; /* 22 */ + int16_t mag_x; /* 24 */ + int16_t mag_y; /* 26 */ + int16_t mag_z; /* 28 */ + int16_t accel; /* 30 */ + } sensor; /* 32 */ + /* AO_LOG_TEMP_VOLT */ + struct { + int16_t v_batt; /* 4 */ + int16_t v_pbatt; /* 6 */ + int16_t n_sense; /* 8 */ + int16_t sense[10]; /* 10 */ + uint16_t pyro; /* 30 */ + } volt; /* 32 */ + /* AO_LOG_GPS_TIME */ + struct { + int32_t latitude; /* 4 */ + int32_t longitude; /* 8 */ + int16_t altitude; /* 12 */ + uint8_t hour; /* 14 */ + uint8_t minute; /* 15 */ + uint8_t second; /* 16 */ + uint8_t flags; /* 17 */ + uint8_t year; /* 18 */ + uint8_t month; /* 19 */ + uint8_t day; /* 20 */ + uint8_t course; /* 21 */ + uint16_t ground_speed; /* 22 */ + int16_t climb_rate; /* 24 */ + uint8_t pdop; /* 26 */ + uint8_t hdop; /* 27 */ + uint8_t vdop; /* 28 */ + uint8_t mode; /* 29 */ + } gps; /* 30 */ + /* AO_LOG_GPS_SAT */ + struct { + uint16_t channels; /* 4 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[12]; /* 6 */ + } gps_sat; /* 30 */ + } u; +}; + +struct ao_log_metrum { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + uint32_t ground_temp; /* 12 */ + } flight; /* 16 */ + /* AO_LOG_STATE */ + struct { + uint16_t state; /* 4 */ + uint16_t reason; /* 6 */ + } state; /* 8 */ + /* AO_LOG_SENSOR */ + struct { + uint32_t pres; /* 4 */ + uint32_t temp; /* 8 */ + int16_t accel; /* 12 */ + } sensor; /* 14 */ + /* AO_LOG_TEMP_VOLT */ + struct { + int16_t v_batt; /* 4 */ + int16_t sense_a; /* 6 */ + int16_t sense_m; /* 8 */ + } volt; /* 10 */ + /* AO_LOG_GPS_POS */ + struct { + int32_t latitude; /* 4 */ + int32_t longitude; /* 8 */ + int16_t altitude; /* 12 */ + } gps; /* 14 */ + /* AO_LOG_GPS_TIME */ + struct { + uint8_t hour; /* 4 */ + uint8_t minute; /* 5 */ + uint8_t second; /* 6 */ + uint8_t flags; /* 7 */ + uint8_t year; /* 8 */ + uint8_t month; /* 9 */ + uint8_t day; /* 10 */ + uint8_t pad; /* 11 */ + } gps_time; /* 12 */ + /* AO_LOG_GPS_SAT (up to three packets) */ + struct { + uint8_t channels; /* 4 */ + uint8_t more; /* 5 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[4]; /* 6 */ + } gps_sat; /* 14 */ + uint8_t raw[12]; /* 4 */ + } u; /* 16 */ +}; + +struct ao_log_mini { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + uint16_t r6; + uint32_t ground_pres; /* 8 */ + } flight; + /* AO_LOG_STATE */ + struct { + uint16_t state; /* 4 */ + uint16_t reason; /* 6 */ + } state; + /* AO_LOG_SENSOR */ + struct { + uint8_t pres[3]; /* 4 */ + uint8_t temp[3]; /* 7 */ + int16_t sense_a; /* 10 */ + int16_t sense_m; /* 12 */ + int16_t v_batt; /* 14 */ + } sensor; /* 16 */ + } u; /* 16 */ +}; /* 16 */ + +#define ao_log_pack24(dst,value) do { \ + (dst)[0] = (value); \ + (dst)[1] = (value) >> 8; \ + (dst)[2] = (value) >> 16; \ + } while (0) + +/* Write a record to the eeprom log */ +uint8_t +ao_log_data(__xdata struct ao_log_record *log) __reentrant; + +uint8_t +ao_log_mega(__xdata struct ao_log_mega *log) __reentrant; + +uint8_t +ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant; + +uint8_t +ao_log_mini(__xdata struct ao_log_mini *log) __reentrant; + +void +ao_log_flush(void); + +void +ao_gps_report_metrum_init(void); + +#endif /* _AO_LOG_H_ */ diff --git a/src/kernel/ao_log_big.c b/src/kernel/ao_log_big.c new file mode 100644 index 00000000..db01f46c --- /dev/null +++ b/src/kernel/ao_log_big.c @@ -0,0 +1,162 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_record log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_FULL; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_record); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_data(__xdata struct ao_log_record *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_record)); + ao_log_current_pos += sizeof (struct ao_log_record); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_records fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_record))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#define AO_OTHER_INTERVAL 32 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor, next_other; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; +#if HAS_ACCEL + log.u.flight.ground_accel = ao_ground_accel; +#endif + log.u.flight.flight = ao_flight_number; + ao_log_data(&log); + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_sample_data); + next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_sample_data) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; + log.u.sensor.accel = ao_data_ring[ao_log_data_pos].adc.accel; + log.u.sensor.pres = ao_data_ring[ao_log_data_pos].adc.pres; + ao_log_data(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + if ((int16_t) (log.tick - next_other) >= 0) { + log.type = AO_LOG_TEMP_VOLT; + log.u.temp_volt.temp = ao_data_ring[ao_log_data_pos].adc.temp; + log.u.temp_volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + ao_log_data(&log); + log.type = AO_LOG_DEPLOY; + log.u.deploy.drogue = ao_data_ring[ao_log_data_pos].adc.sense_d; + log.u.deploy.main = ao_data_ring[ao_log_data_pos].adc.sense_m; + ao_log_data(&log); + next_other = log.tick + AO_OTHER_INTERVAL; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_sample_tick; + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_data(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_record))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_mega.c b/src/kernel/ao_log_mega.c new file mode 100644 index 00000000..768947d5 --- /dev/null +++ b/src/kernel/ao_log_mega.c @@ -0,0 +1,199 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include +#include +#include + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_mega log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMEGA; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_mega); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_mega(__xdata struct ao_log_mega *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_mega)); + ao_log_current_pos += sizeof (struct ao_log_mega); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +#if HAS_ADC +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_megas fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mega))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#define AO_OTHER_INTERVAL 32 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor, next_other; + uint8_t i; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + +#if HAS_FLIGHT + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; +#if HAS_ACCEL + log.u.flight.ground_accel = ao_ground_accel; +#endif +#if HAS_GYRO + log.u.flight.ground_accel_along = ao_ground_accel_along; + log.u.flight.ground_accel_across = ao_ground_accel_across; + log.u.flight.ground_accel_through = ao_ground_accel_through; + log.u.flight.ground_pitch = ao_ground_pitch; + log.u.flight.ground_yaw = ao_ground_yaw; + log.u.flight.ground_roll = ao_ground_roll; +#endif + log.u.flight.ground_pres = ao_ground_pres; + log.u.flight.flight = ao_flight_number; + ao_log_mega(&log); +#endif + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; +#if HAS_MS5607 + log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; + log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; +#endif +#if HAS_MPU6000 + log.u.sensor.accel_x = ao_data_ring[ao_log_data_pos].mpu6000.accel_x; + log.u.sensor.accel_y = ao_data_ring[ao_log_data_pos].mpu6000.accel_y; + log.u.sensor.accel_z = ao_data_ring[ao_log_data_pos].mpu6000.accel_z; + log.u.sensor.gyro_x = ao_data_ring[ao_log_data_pos].mpu6000.gyro_x; + log.u.sensor.gyro_y = ao_data_ring[ao_log_data_pos].mpu6000.gyro_y; + log.u.sensor.gyro_z = ao_data_ring[ao_log_data_pos].mpu6000.gyro_z; +#endif +#if HAS_HMC5883 + log.u.sensor.mag_x = ao_data_ring[ao_log_data_pos].hmc5883.x; + log.u.sensor.mag_y = ao_data_ring[ao_log_data_pos].hmc5883.y; + log.u.sensor.mag_z = ao_data_ring[ao_log_data_pos].hmc5883.z; +#endif + log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); + ao_log_mega(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + if ((int16_t) (log.tick - next_other) >= 0) { + log.type = AO_LOG_TEMP_VOLT; + log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + log.u.volt.v_pbatt = ao_data_ring[ao_log_data_pos].adc.v_pbatt; + log.u.volt.n_sense = AO_ADC_NUM_SENSE; + for (i = 0; i < AO_ADC_NUM_SENSE; i++) + log.u.volt.sense[i] = ao_data_ring[ao_log_data_pos].adc.sense[i]; + log.u.volt.pyro = ao_pyro_fired; + ao_log_mega(&log); + next_other = log.tick + AO_OTHER_INTERVAL; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } +#if HAS_FLIGHT + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_time(); + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_mega(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } +#endif + + ao_log_flush(); + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} +#endif + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_mega))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_metrum.c b/src/kernel/ao_log_metrum.c new file mode 100644 index 00000000..9b17adc2 --- /dev/null +++ b/src/kernel/ao_log_metrum.c @@ -0,0 +1,176 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include +#include +#include + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_metrum log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRUM; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_metrum); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_metrum)); + ao_log_current_pos += sizeof (struct ao_log_metrum); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +#if HAS_ADC +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_metrums fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_metrum))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#define AO_OTHER_INTERVAL 32 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor, next_other; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + +#if HAS_FLIGHT + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; +#if HAS_ACCEL + log.u.flight.ground_accel = ao_ground_accel; +#endif + log.u.flight.ground_pres = ao_ground_pres; + log.u.flight.flight = ao_flight_number; + ao_log_metrum(&log); +#endif + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; +#if HAS_MS5607 + log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; + log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; +#endif +#if HAS_ACCEL + log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); +#endif + ao_log_metrum(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + if ((int16_t) (log.tick - next_other) >= 0) { + log.type = AO_LOG_TEMP_VOLT; + log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + log.u.volt.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; + log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; + ao_log_metrum(&log); + next_other = log.tick + AO_OTHER_INTERVAL; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } +#if HAS_FLIGHT + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_time(); + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_metrum(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } +#endif + + ao_log_flush(); + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} +#endif + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_metrum))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_micro.c b/src/kernel/ao_log_micro.c new file mode 100644 index 00000000..d665efb5 --- /dev/null +++ b/src/kernel/ao_log_micro.c @@ -0,0 +1,121 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include + +static uint16_t ao_log_offset = STARTING_LOG_OFFSET; + +void +ao_log_micro_save(void) +{ + uint16_t n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t); + ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); + ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); + ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); +} + +void +ao_log_micro_restore(void) +{ + ao_eeprom_read(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); + ao_eeprom_read(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); +} + +void +ao_log_micro_data(void) +{ + uint16_t low_bits = pa; + + if (ao_log_offset < MAX_LOG_OFFSET) { + ao_eeprom_write(ao_log_offset, &low_bits, sizeof (low_bits)); + ao_log_offset += sizeof (low_bits); + } +} + +#define POLY 0x8408 + +static uint16_t +ao_log_micro_crc(uint16_t crc, uint8_t byte) +{ + uint8_t i; + + for (i = 0; i < 8; i++) { + if ((crc & 0x0001) ^ (byte & 0x0001)) + crc = (crc >> 1) ^ POLY; + else + crc = crc >> 1; + byte >>= 1; + } + return crc; +} + +static void +ao_log_hex_nibble(uint8_t b) +{ + if (b < 10) + ao_async_byte('0' + b); + else + ao_async_byte('a' - 10 + b); +} + +static void +ao_log_hex(uint8_t b) +{ + ao_log_hex_nibble(b>>4); + ao_log_hex_nibble(b&0xf); +} + +static void +ao_log_newline(void) +{ + ao_async_byte('\r'); + ao_async_byte('\n'); +} + +void +ao_log_micro_dump(void) +{ + uint16_t n_samples; + uint16_t nbytes; + uint8_t byte; + uint16_t b; + uint16_t crc = 0xffff; + + ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); + if (n_samples == 0xffff) + n_samples = 0; + nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples; + ao_async_start(); + ao_async_byte('M'); + ao_async_byte('P'); + for (b = 0; b < nbytes; b++) { + if ((b & 0xf) == 0) + ao_log_newline(); + ao_eeprom_read(b, &byte, 1); + ao_log_hex(byte); + crc = ao_log_micro_crc(crc, byte); + } + ao_log_newline(); + crc = ~crc; + ao_log_hex(crc >> 8); + ao_log_hex(crc); + ao_log_newline(); + ao_async_stop(); +} diff --git a/src/kernel/ao_log_micro.h b/src/kernel/ao_log_micro.h new file mode 100644 index 00000000..976852ee --- /dev/null +++ b/src/kernel/ao_log_micro.h @@ -0,0 +1,39 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_LOG_MICRO_H_ +#define _AO_LOG_MICRO_H_ + +#define PA_GROUND_OFFSET 0 +#define PA_MIN_OFFSET 4 +#define N_SAMPLES_OFFSET 8 +#define STARTING_LOG_OFFSET 10 +#define MAX_LOG_OFFSET 512 + +void +ao_log_micro_save(void); + +void +ao_log_micro_restore(void); + +void +ao_log_micro_data(void); + +void +ao_log_micro_dump(void); + +#endif /* _AO_LOG_MICRO_H_ */ diff --git a/src/kernel/ao_log_mini.c b/src/kernel/ao_log_mini.c new file mode 100644 index 00000000..29e3bd9f --- /dev/null +++ b/src/kernel/ao_log_mini.c @@ -0,0 +1,162 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include +#include +#include + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_mini log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_mini); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_mini(__xdata struct ao_log_mini *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_mini)); + ao_log_current_pos += sizeof (struct ao_log_mini); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_minis fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mini))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + +#if HAS_FLIGHT + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; + log.u.flight.flight = ao_flight_number; + log.u.flight.ground_pres = ao_ground_pres; + ao_log_mini(&log); +#endif + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; + ao_log_pack24(log.u.sensor.pres, + ao_data_ring[ao_log_data_pos].ms5607_raw.pres); + ao_log_pack24(log.u.sensor.temp, + ao_data_ring[ao_log_data_pos].ms5607_raw.temp); + log.u.sensor.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; + log.u.sensor.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; + log.u.sensor.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + ao_log_mini(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } +#if HAS_FLIGHT + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_time(); + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_mini(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } +#endif + + ao_log_flush(); + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_mini))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_single.c b/src/kernel/ao_log_single.c new file mode 100644 index 00000000..3f6235a6 --- /dev/null +++ b/src/kernel/ao_log_single.c @@ -0,0 +1,198 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +/* + * ao_log_single.c + * + * Stores a sequence of fixed-size (32 byte) chunks + * without splitting memory up into separate flights + */ + +#include "ao.h" +#include "ao_product.h" + +static __xdata struct ao_task ao_log_single_task; + +__xdata uint8_t ao_log_running; +__xdata uint8_t ao_log_mutex; +__pdata uint32_t ao_log_start_pos; +__pdata uint32_t ao_log_end_pos; +__pdata uint32_t ao_log_current_pos; + +__xdata union ao_log_single ao_log_single_write_data; +__xdata union ao_log_single ao_log_single_read_data; + +uint8_t +ao_log_single_write(void) +{ + uint8_t wrote = 0; + + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_single_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + &ao_log_single_write_data, + AO_LOG_SINGLE_SIZE); + ao_log_current_pos += AO_LOG_SINGLE_SIZE; + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_single_valid(void) +{ + __xdata uint8_t *d = ao_log_single_read_data.bytes; + uint8_t i; + for (i = 0; i < AO_LOG_SINGLE_SIZE; i++) + if (*d++ != 0xff) + return 1; + return 0; +} + +uint8_t +ao_log_single_read(uint32_t pos) +{ + if (!ao_storage_read(pos, &ao_log_single_read_data, AO_LOG_SINGLE_SIZE)) + return 0; + return ao_log_single_valid(); +} + +void +ao_log_single_start(void) +{ + if (!ao_log_running) { + ao_log_running = 1; + ao_wakeup(&ao_log_running); + } +} + +void +ao_log_single_stop(void) +{ + if (ao_log_running) { + ao_log_running = 0; + } +} + +void +ao_log_single_restart(void) +{ + /* Find end of data */ + ao_log_end_pos = ao_storage_config; + for (ao_log_current_pos = 0; + ao_log_current_pos < ao_storage_config; + ao_log_current_pos += ao_storage_block) + { + if (!ao_log_single_read(ao_log_current_pos)) + break; + } + if (ao_log_current_pos > 0) { + ao_log_current_pos -= ao_storage_block; + for (; ao_log_current_pos < ao_storage_config; + ao_log_current_pos += sizeof (struct ao_log_telescience)) + { + if (!ao_log_single_read(ao_log_current_pos)) + break; + } + } +} + +void +ao_log_single_set(void) +{ + printf("Logging currently %s\n", ao_log_running ? "on" : "off"); + ao_cmd_hex(); + if (ao_cmd_status == ao_cmd_success) { + if (ao_cmd_lex_i) { + printf("Logging from %ld to %ld\n", ao_log_current_pos, ao_log_end_pos); + ao_log_single_start(); + } else { + printf ("Log stopped at %ld\n", ao_log_current_pos); + ao_log_single_stop(); + } + } + ao_cmd_status = ao_cmd_success; +} + +void +ao_log_single_delete(void) +{ + uint32_t pos; + + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + if (ao_cmd_lex_i != 1) { + ao_cmd_status = ao_cmd_syntax_error; + printf("No such flight: %d\n", ao_cmd_lex_i); + return; + } + ao_log_single_stop(); + for (pos = 0; pos < ao_storage_config; pos += ao_storage_block) { + if (!ao_log_single_read(pos)) + break; + ao_storage_erase(pos); + } + ao_log_current_pos = ao_log_start_pos = 0; + if (pos == 0) + printf("No such flight: %d\n", ao_cmd_lex_i); + else + printf ("Erased\n"); +} + +uint8_t +ao_log_full(void) +{ + return ao_log_current_pos >= ao_log_end_pos; +} + +uint8_t +ao_log_present(void) +{ + return ao_log_single_read(0); +} + +static void +ao_log_single_query(void) +{ + printf("Logging enabled: %d\n", ao_log_running); + printf("Log start: %ld\n", ao_log_start_pos); + printf("Log cur: %ld\n", ao_log_current_pos); + printf("Log end: %ld\n", ao_log_end_pos); + ao_log_single_extra_query(); +} + +const struct ao_cmds ao_log_single_cmds[] = { + { ao_log_single_set, "L <0 off, 1 on>\0Set logging" }, + { ao_log_single_list, "l\0List stored logs" }, + { ao_log_single_delete, "d 1\0Delete all stored logs" }, + { ao_log_single_query, "q\0Query log status" }, + { 0, NULL }, +}; + +void +ao_log_single_init(void) +{ + ao_log_running = 0; + + ao_cmd_register(&ao_log_single_cmds[0]); + + ao_add_task(&ao_log_single_task, ao_log_single, "log"); +} diff --git a/src/kernel/ao_log_telem.c b/src/kernel/ao_log_telem.c new file mode 100644 index 00000000..095aca37 --- /dev/null +++ b/src/kernel/ao_log_telem.c @@ -0,0 +1,131 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRY; + +static __data uint8_t ao_log_monitor_pos; +__pdata enum ao_flight_state ao_flight_state; +__xdata int16_t ao_max_height; /* max of ao_height */ +__pdata int16_t sense_d, sense_m; +__pdata uint8_t ao_igniter_present; + +static void +ao_log_telem_track() { + if (ao_monitoring == sizeof (union ao_telemetry_all)) { + switch (ao_log_single_write_data.telemetry.generic.type) { + case AO_TELEMETRY_SENSOR_TELEMETRUM: + case AO_TELEMETRY_SENSOR_TELEMINI: + /* fall through ... */ + case AO_TELEMETRY_SENSOR_TELENANO: + if (ao_log_single_write_data.telemetry.generic.type == AO_TELEMETRY_SENSOR_TELENANO) { + ao_igniter_present = 0; + } else { + sense_d = ao_log_single_write_data.telemetry.sensor.sense_d; + sense_m = ao_log_single_write_data.telemetry.sensor.sense_m; + ao_igniter_present = 1; + } + if (ao_log_single_write_data.telemetry.sensor.height > ao_max_height) { + ao_max_height = ao_log_single_write_data.telemetry.sensor.height; + } + if (ao_log_single_write_data.telemetry.sensor.state != ao_flight_state) { + ao_flight_state = ao_log_single_write_data.telemetry.sensor.state; + if (ao_flight_state == ao_flight_pad) + ao_max_height = 0; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + } + } +} + +enum ao_igniter_status +ao_igniter_status(enum ao_igniter igniter) +{ + int16_t value; + + switch (igniter) { + case ao_igniter_drogue: + value = sense_d; + break; + case ao_igniter_main: + value = sense_m; + break; + default: + value = 0; + break; + } + if (value < AO_IGNITER_OPEN) + return ao_igniter_open; + else if (value > AO_IGNITER_CLOSED) + return ao_igniter_ready; + else + return ao_igniter_unknown; +} + +void +ao_log_single(void) +{ + ao_storage_setup(); + + /* This can take a while, so let the rest + * of the system finish booting before we start + */ + ao_delay(AO_SEC_TO_TICKS(2)); + + ao_log_running = 1; + ao_log_single_restart(); + ao_flight_state = ao_flight_startup; + ao_monitor_set(sizeof(struct ao_telemetry_generic)); + + for (;;) { + while (!ao_log_running) + ao_sleep(&ao_log_running); + + ao_log_monitor_pos = ao_monitor_head; + while (ao_log_running) { + /* Write samples to EEPROM */ + while (ao_log_monitor_pos != ao_monitor_head) { + ao_xmemcpy(&ao_log_single_write_data.telemetry, + &ao_monitor_ring[ao_log_monitor_pos], + AO_LOG_SINGLE_SIZE); + ao_log_single_write(); + ao_log_monitor_pos = ao_monitor_ring_next(ao_log_monitor_pos); + ao_log_telem_track(); + } + /* Wait for more telemetry data to arrive */ + ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); + } + } +} + +void +ao_log_single_list(void) +{ + if (ao_log_current_pos != 0) + printf("flight 1 start %x end %x\n", + 0, + (uint16_t) ((ao_log_current_pos + 0xff) >> 8)); + printf ("done\n"); +} + +void +ao_log_single_extra_query(void) +{ +} diff --git a/src/kernel/ao_log_telescience.c b/src/kernel/ao_log_telescience.c new file mode 100644 index 00000000..002a10bd --- /dev/null +++ b/src/kernel/ao_log_telescience.c @@ -0,0 +1,119 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_product.h" +#include "ao_log.h" +#include "ao_companion.h" + +static uint8_t ao_log_data_pos; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELESCIENCE; + +static void +ao_log_telescience_csum(void) __reentrant +{ + __xdata uint8_t *b = ao_log_single_write_data.bytes; + uint8_t sum = 0x5a; + uint8_t i; + + ao_log_single_write_data.telescience.csum = 0; + for (i = 0; i < sizeof (struct ao_log_telescience); i++) + sum += *b++; + ao_log_single_write_data.telescience.csum = -sum; +} + +void +ao_log_single(void) +{ + ao_storage_setup(); + + /* This can take a while, so let the rest + * of the system finish booting before we start + */ + ao_delay(AO_SEC_TO_TICKS(10)); + + ao_log_single_restart(); + for (;;) { + while (!ao_log_running) + ao_sleep(&ao_log_running); + + ao_log_start_pos = ao_log_current_pos; + ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_START; + ao_log_single_write_data.telescience.tick = ao_time(); + ao_log_single_write_data.telescience.adc[0] = ao_companion_command.serial; + ao_log_single_write_data.telescience.adc[1] = ao_companion_command.flight; + ao_log_telescience_csum(); + ao_log_single_write(); + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_DATA; + while (ao_log_running) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + ao_log_single_write_data.telescience.tick = ao_data_ring[ao_log_data_pos].tick; + memcpy(&ao_log_single_write_data.telescience.adc, (void *) ao_data_ring[ao_log_data_pos].adc.adc, + AO_LOG_TELESCIENCE_NUM_ADC * sizeof (uint16_t)); + ao_log_telescience_csum(); + ao_log_single_write(); + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } + /* Wait for more ADC data to arrive */ + ao_sleep((void *) &ao_data_head); + } + memset(&ao_log_single_write_data.telescience.adc, '\0', sizeof (ao_log_single_write_data.telescience.adc)); + } +} + +void +ao_log_single_list(void) +{ + uint32_t pos; + uint32_t start = 0; + uint8_t flight = 0; + + for (pos = 0; ; pos += sizeof (struct ao_log_telescience)) { + if (pos >= ao_storage_config || + !ao_log_single_read(pos) || + ao_log_single_read_data.telescience.type == AO_LOG_TELESCIENCE_START) + { + if (pos != start) { + printf("flight %d start %x end %x\n", + flight, + (uint16_t) (start >> 8), + (uint16_t) ((pos + 0xff) >> 8)); flush(); + } + if (ao_log_single_read_data.telescience.type != AO_LOG_TELESCIENCE_START) + break; + start = pos; + flight++; + } + } + printf ("done\n"); +} + +void +ao_log_single_extra_query(void) +{ + printf("log data tick: %04x\n", ao_log_single_write_data.telescience.tick); + printf("TM data tick: %04x\n", ao_log_single_write_data.telescience.tm_tick); + printf("TM state: %d\n", ao_log_single_write_data.telescience.tm_state); + printf("TM serial: %d\n", ao_companion_command.serial); + printf("TM flight: %d\n", ao_companion_command.flight); +} diff --git a/src/kernel/ao_log_tiny.c b/src/kernel/ao_log_tiny.c new file mode 100644 index 00000000..67767dc9 --- /dev/null +++ b/src/kernel/ao_log_tiny.c @@ -0,0 +1,161 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +static __data uint16_t ao_log_tiny_interval; + +#define AO_LOG_TINY_INTERVAL_DEFAULT AO_MS_TO_TICKS(1000) +#if USE_FAST_ASCENT_LOG +#define AO_LOG_TINY_INTERVAL_ASCENT AO_MS_TO_TICKS(100) +#define AO_PAD_RING 8 +#else +#define AO_LOG_TINY_INTERVAL_ASCENT AO_LOG_TINY_INTERVAL_DEFAULT +#define AO_PAD_RING 2 +#endif + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TINY; + +void +ao_log_tiny_set_interval(uint16_t ticks) +{ + ao_log_tiny_interval = ticks; +} + + +static void ao_log_tiny_data(uint16_t d) +{ + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + ao_storage_write(ao_log_current_pos, DATA_TO_XDATA(&d), 2); + ao_log_current_pos += 2; + } +} + +static __xdata uint16_t ao_log_pad_ring[AO_PAD_RING]; +static __pdata uint8_t ao_log_pad_ring_pos; + +#define ao_pad_ring_next(n) (((n) + 1) & (AO_PAD_RING - 1)) + +static void ao_log_tiny_queue(uint16_t d) +{ + ao_log_pad_ring[ao_log_pad_ring_pos] = d; + ao_log_pad_ring_pos = ao_pad_ring_next(ao_log_pad_ring_pos); +} + +static void ao_log_tiny_start(void) +{ + uint8_t p; + uint16_t d; + + ao_log_tiny_data(ao_flight_number); + ao_log_tiny_data(ao_ground_pres); + p = ao_log_pad_ring_pos; + do { + d = ao_log_pad_ring[p]; + /* + * ignore unwritten slots + */ + if (d) + ao_log_tiny_data(d); + p = ao_pad_ring_next(p); + } while (p != ao_log_pad_ring_pos); +} + +void +ao_log(void) +{ + uint16_t last_time; + uint16_t now; + enum ao_flight_state ao_log_tiny_state; + int32_t sum; + int16_t count; + uint8_t ao_log_data; + uint8_t ao_log_started = 0; + + ao_storage_setup(); + + ao_log_scan(); + + ao_log_tiny_state = ao_flight_invalid; + ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT; + sum = 0; + count = 0; + ao_log_data = ao_sample_data; + last_time = ao_time(); + for (;;) { + + /* + * Add in pending sample data + */ + ao_sleep(DATA_TO_XDATA(&ao_sample_data)); + while (ao_log_data != ao_sample_data) { + sum += ao_data_pres(&ao_data_ring[ao_log_data]); + count++; + ao_log_data = ao_data_ring_next(ao_log_data); + } + if (ao_log_running) { + if (!ao_log_started) { + ao_log_tiny_start(); + ao_log_started = 1; + } + if (ao_flight_state != ao_log_tiny_state) { + ao_log_tiny_data(ao_flight_state | 0x8000); + ao_log_tiny_state = ao_flight_state; + ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_DEFAULT; +#if AO_LOG_TINY_INTERVAL_ASCENT != AO_LOG_TINY_INTERVAL_DEFAULT + if (ao_log_tiny_state <= ao_flight_coast) + ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT; +#endif + if (ao_log_tiny_state == ao_flight_landed) + ao_log_stop(); + } + } + + /* Stop logging when told to */ + if (!ao_log_running && ao_log_started) + ao_exit(); + + /* + * Write out the sample when finished + */ + now = ao_time(); + if ((int16_t) (now - (last_time + ao_log_tiny_interval)) >= 0 && count) { + count = sum / count; + if (ao_log_started) + ao_log_tiny_data(count); + else + ao_log_tiny_queue(count); + sum = 0; + count = 0; + last_time = now; + } + } +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + static __xdata uint16_t flight; + + (void) slot; + ao_storage_read(0, &flight, 2); + if (flight == 0xffff) + flight = 0; + return flight; +} diff --git a/src/kernel/ao_microflight.c b/src/kernel/ao_microflight.c new file mode 100644 index 00000000..f680e400 --- /dev/null +++ b/src/kernel/ao_microflight.c @@ -0,0 +1,143 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include +#endif +#include +#include + +uint32_t pa; +uint32_t pa_ground; +uint32_t pa_min; + +static void +ao_microsample(void) +{ + ao_pa_get(); + ao_microkalman_predict(); + ao_microkalman_correct(); +} + +#define NUM_PA_HIST (GROUND_AVG) + +#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) + +static uint32_t pa_hist[NUM_PA_HIST]; + +void +ao_microflight(void) +{ + int16_t sample_count; + uint16_t time; + uint32_t pa_interval_min, pa_interval_max; + int32_t pa_diff; + uint8_t h, i; + uint8_t accel_lock = 0; + uint32_t pa_sum = 0; + + /* Wait for motion, averaging values to get ground pressure */ + + time = ao_time(); + ao_pa_get(); + ao_microkalman_init(); + pa_ground = pa; + sample_count = 0; + h = 0; + for (;;) { + time += SAMPLE_SLEEP; + if ((sample_count & 0x1f) == 0) + ao_led_on(AO_LED_REPORT); + ao_delay_until(time); + ao_microsample(); + if ((sample_count & 0x1f) == 0) + ao_led_off(AO_LED_REPORT); + pa_hist[h] = pa; + h = SKIP_PA_HIST(h,1); + pa_diff = pa_ground - ao_pa; + + /* Check for a significant pressure change */ + if (pa_diff > BOOST_DETECT) + break; + + if (sample_count < GROUND_AVG * 2) { + if (sample_count < GROUND_AVG) + pa_sum += pa; + ++sample_count; + } else { + pa_ground = pa_sum >> GROUND_AVG_SHIFT; + pa_sum = 0; + sample_count = 0; + } + } + + /* Go back and find the last sample close to the ground */ + pa_min = pa_ground - LAND_DETECT; + for (i = SKIP_PA_HIST(h,-2); i != SKIP_PA_HIST(h,2); i = SKIP_PA_HIST(i,-2)) { + if (pa_hist[i] >= pa_min) + break; + } + + /* Log the remaining samples so we get a complete history since leaving the ground */ + for (; i != h; i = SKIP_PA_HIST(i,2)) { + pa = pa_hist[i]; + ao_log_micro_data(); + } + + /* Now sit around until the pressure is stable again and record the max */ + + sample_count = 0; + pa_min = ao_pa; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + for (;;) { + time += SAMPLE_SLEEP; + ao_delay_until(time); + if ((sample_count & 3) == 0) + ao_led_on(AO_LED_REPORT); + ao_microsample(); + if ((sample_count & 3) == 0) + ao_led_off(AO_LED_REPORT); + if (sample_count & 1) + ao_log_micro_data(); + + /* If accelerating upwards, don't look for min pressure */ + if (ao_pa_accel < ACCEL_LOCK_PA) + accel_lock = ACCEL_LOCK_TIME; + else if (accel_lock) + --accel_lock; + else if (ao_pa < pa_min) + pa_min = ao_pa; + + if (sample_count == (GROUND_AVG - 1)) { + pa_diff = pa_interval_max - pa_interval_min; + + /* Check to see if the pressure is now stable */ + if (pa_diff < LAND_DETECT) + break; + sample_count = 0; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + } else { + if (ao_pa < pa_interval_min) + pa_interval_min = ao_pa; + if (ao_pa > pa_interval_max) + pa_interval_max = ao_pa; + ++sample_count; + } + } +} diff --git a/src/kernel/ao_microkalman.c b/src/kernel/ao_microkalman.c new file mode 100644 index 00000000..0684ea2b --- /dev/null +++ b/src/kernel/ao_microkalman.c @@ -0,0 +1,74 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include +#endif +#include + +#define FIX_BITS 16 + +#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define from_fix8(x) ((x) >> 8) +#define from_fix(x) ((x) >> 16) +#define fix8_to_fix16(x) ((x) << 8) +#define fix16_to_fix8(x) ((x) >> 8) + +#include + +/* Basic time step (96ms) */ +#define AO_MK_STEP to_fix16(0.096) +/* step ** 2 / 2 */ +#define AO_MK_STEP_2_2 to_fix16(0.004608) + +uint32_t ao_k_pa; /* 24.8 fixed point */ +int32_t ao_k_pa_speed; /* 16.16 fixed point */ +int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +uint32_t ao_pa; /* integer portion */ +int16_t ao_pa_speed; /* integer portion */ +int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void) +{ + ao_pa = pa; + ao_k_pa = pa << 8; +} + +void +ao_microkalman_predict(void) +{ + ao_k_pa += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2); + ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP; +} + +void +ao_microkalman_correct(void) +{ + int16_t e; /* Height error in Pa */ + + e = pa - from_fix8(ao_k_pa); + + ao_k_pa += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10); + ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10; + ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10; + ao_pa = from_fix8(ao_k_pa); + ao_pa_speed = from_fix(ao_k_pa_speed); + ao_pa_accel = from_fix(ao_k_pa_accel); +} diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c new file mode 100644 index 00000000..18f170b4 --- /dev/null +++ b/src/kernel/ao_monitor.c @@ -0,0 +1,308 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_telem.h" +#include "ao_flight.h" + +#if !HAS_MONITOR +#error Must define HAS_MONITOR to 1 +#endif + +#ifndef LEGACY_MONITOR +#error Must define LEGACY_MONITOR +#endif + +#ifndef HAS_MONITOR_PUT +#define HAS_MONITOR_PUT 1 +#endif + +#ifndef AO_MONITOR_LED +#error Must define AO_MONITOR_LED +#endif + +__data uint8_t ao_monitoring; +static __data uint8_t ao_monitor_disabled; +static __data uint8_t ao_internal_monitoring; +static __data uint8_t ao_external_monitoring; + +__xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; + +__data uint8_t ao_monitor_head; + +static void +_ao_monitor_adjust(void) +{ + if (ao_monitoring) + ao_radio_recv_abort(); + if (ao_monitor_disabled) + ao_monitoring = 0; + else { + if (ao_external_monitoring) + ao_monitoring = ao_external_monitoring; + else + ao_monitoring = ao_internal_monitoring; + } + ao_wakeup(DATA_TO_XDATA(&ao_monitoring)); +} + +void +ao_monitor_get(void) +{ + uint8_t size; + + for (;;) { + switch (ao_monitoring) { + case 0: + ao_sleep(DATA_TO_XDATA(&ao_monitoring)); + continue; +#if LEGACY_MONITOR + case AO_MONITORING_ORIG: + size = sizeof (struct ao_telemetry_orig_recv); + break; +#endif + default: + if (ao_monitoring > AO_MAX_TELEMETRY) + ao_monitoring = AO_MAX_TELEMETRY; + size = ao_monitoring; + break; + } + if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2, 0)) + continue; + ao_monitor_head = ao_monitor_ring_next(ao_monitor_head); + ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); + } +} + +#if AO_MONITOR_LED +__xdata struct ao_task ao_monitor_blink_task; + +void +ao_monitor_blink(void) +{ + for (;;) { + ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); + ao_led_for(AO_MONITOR_LED, AO_MS_TO_TICKS(100)); + } +} +#endif + +#if HAS_MONITOR_PUT + +static const char xdigit[16] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' +}; + +#define hex(c) do { putchar(xdigit[(c) >> 4]); putchar(xdigit[(c)&0xf]); } while (0) + +void +ao_monitor_put(void) +{ +#if LEGACY_MONITOR + __xdata char callsign[AO_MAX_CALLSIGN+1]; + int16_t rssi; +#endif + uint8_t ao_monitor_tail; + uint8_t state; + uint8_t sum, byte; + __xdata union ao_monitor *m; + +#define recv_raw ((m->raw)) +#define recv_orig ((m->orig)) +#define recv_tiny ((m->tiny)) + + ao_monitor_tail = ao_monitor_head; + for (;;) { + while (!ao_external_monitoring) + ao_sleep(DATA_TO_XDATA(&ao_external_monitoring)); + while (ao_monitor_tail == ao_monitor_head && ao_external_monitoring) + ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); + if (!ao_external_monitoring) + continue; + m = &ao_monitor_ring[ao_monitor_tail]; + ao_monitor_tail = ao_monitor_ring_next(ao_monitor_tail); + switch (ao_monitoring) { + case 0: + break; +#if LEGACY_MONITOR + case AO_MONITORING_ORIG: + state = recv_orig.telemetry_orig.flight_state; + + rssi = (int16_t) AO_RSSI_FROM_RADIO(recv_orig.rssi); + ao_xmemcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN); + if (state > ao_flight_invalid) + state = ao_flight_invalid; + if (recv_orig.status & PKT_APPEND_STATUS_1_CRC_OK) { + + /* General header fields */ + printf(AO_TELEM_VERSION " %d " + AO_TELEM_CALL " %s " + AO_TELEM_SERIAL " %d " + AO_TELEM_FLIGHT " %d " + AO_TELEM_RSSI " %d " + AO_TELEM_STATE " %s " + AO_TELEM_TICK " %d ", + AO_TELEMETRY_VERSION, + callsign, + recv_orig.telemetry_orig.serial, + recv_orig.telemetry_orig.flight, + rssi, + ao_state_names[state], + recv_orig.telemetry_orig.adc.tick); + + /* Raw sensor values */ + printf(AO_TELEM_RAW_ACCEL " %d " + AO_TELEM_RAW_BARO " %d " + AO_TELEM_RAW_THERMO " %d " + AO_TELEM_RAW_BATT " %d " + AO_TELEM_RAW_DROGUE " %d " + AO_TELEM_RAW_MAIN " %d ", + recv_orig.telemetry_orig.adc.accel, + recv_orig.telemetry_orig.adc.pres, + recv_orig.telemetry_orig.adc.temp, + recv_orig.telemetry_orig.adc.v_batt, + recv_orig.telemetry_orig.adc.sense_d, + recv_orig.telemetry_orig.adc.sense_m); + + /* Sensor calibration values */ + printf(AO_TELEM_CAL_ACCEL_GROUND " %d " + AO_TELEM_CAL_BARO_GROUND " %d " + AO_TELEM_CAL_ACCEL_PLUS " %d " + AO_TELEM_CAL_ACCEL_MINUS " %d ", + recv_orig.telemetry_orig.ground_accel, + recv_orig.telemetry_orig.ground_pres, + recv_orig.telemetry_orig.accel_plus_g, + recv_orig.telemetry_orig.accel_minus_g); + + if (recv_orig.telemetry_orig.u.k.unused == 0x8000) { + /* Kalman state values */ + printf(AO_TELEM_KALMAN_HEIGHT " %d " + AO_TELEM_KALMAN_SPEED " %d " + AO_TELEM_KALMAN_ACCEL " %d ", + recv_orig.telemetry_orig.height, + recv_orig.telemetry_orig.u.k.speed, + recv_orig.telemetry_orig.accel); + } else { + /* Ad-hoc flight values */ + printf(AO_TELEM_ADHOC_ACCEL " %d " + AO_TELEM_ADHOC_SPEED " %ld " + AO_TELEM_ADHOC_BARO " %d ", + recv_orig.telemetry_orig.accel, + recv_orig.telemetry_orig.u.flight_vel, + recv_orig.telemetry_orig.height); + } + ao_gps_print(&recv_orig.telemetry_orig.gps); + ao_gps_tracking_print(&recv_orig.telemetry_orig.gps_tracking); + putchar('\n'); +#if HAS_RSSI + ao_rssi_set(rssi); +#endif + } else { + printf("CRC INVALID RSSI %3d\n", rssi); + } + break; +#endif /* LEGACY_MONITOR */ + default: +#if AO_PROFILE + { + extern uint32_t ao_rx_start_tick, ao_rx_packet_tick, ao_rx_done_tick, ao_rx_last_done_tick; + extern uint32_t ao_fec_decode_start, ao_fec_decode_end; + + printf ("between packet: %d\n", ao_rx_start_tick - ao_rx_last_done_tick); + printf ("receive start delay: %d\n", ao_rx_packet_tick - ao_rx_start_tick); + printf ("decode time: %d\n", ao_fec_decode_end - ao_fec_decode_start); + printf ("rx cleanup: %d\n", ao_rx_done_tick - ao_fec_decode_end); + } +#endif + printf("TELEM "); + hex((uint8_t) (ao_monitoring + 2)); + sum = 0x5a; + for (state = 0; state < ao_monitoring + 2; state++) { + byte = recv_raw.packet[state]; + sum += byte; + hex(byte); + } + hex(sum); + putchar ('\n'); +#if HAS_RSSI + if (recv_raw.packet[ao_monitoring + 1] & PKT_APPEND_STATUS_1_CRC_OK) { + rssi = AO_RSSI_FROM_RADIO(recv_raw.packet[ao_monitoring]); + ao_rssi_set(rssi); + } +#endif + break; + } + ao_usb_flush(); + } +} + +__xdata struct ao_task ao_monitor_put_task; +#endif + +__xdata struct ao_task ao_monitor_get_task; + +void +ao_monitor_set(uint8_t monitoring) +{ + ao_internal_monitoring = monitoring; + _ao_monitor_adjust(); +} + +void +ao_monitor_disable(void) +{ + ++ao_monitor_disabled; + _ao_monitor_adjust(); +} + +void +ao_monitor_enable(void) +{ + --ao_monitor_disabled; + _ao_monitor_adjust(); +} + +#if HAS_MONITOR_PUT +static void +set_monitor(void) +{ + ao_cmd_hex(); + ao_external_monitoring = ao_cmd_lex_i; + ao_wakeup(DATA_TO_XDATA(&ao_external_monitoring)); + ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); + _ao_monitor_adjust(); +} + +__code struct ao_cmds ao_monitor_cmds[] = { + { set_monitor, "m <0 off, 1 old, 20 std>\0Set radio monitoring" }, + { 0, NULL }, +}; +#endif + +void +ao_monitor_init(void) __reentrant +{ +#if HAS_MONITOR_PUT + ao_cmd_register(&ao_monitor_cmds[0]); + ao_add_task(&ao_monitor_put_task, ao_monitor_put, "monitor_put"); +#endif + ao_add_task(&ao_monitor_get_task, ao_monitor_get, "monitor_get"); +#if AO_MONITOR_LED + ao_add_task(&ao_monitor_blink_task, ao_monitor_blink, "monitor_blink"); +#endif +} diff --git a/src/kernel/ao_mutex.c b/src/kernel/ao_mutex.c new file mode 100644 index 00000000..952ff462 --- /dev/null +++ b/src/kernel/ao_mutex.c @@ -0,0 +1,41 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +void +ao_mutex_get(__xdata uint8_t *mutex) __reentrant +{ + if (*mutex == ao_cur_task->task_id) + ao_panic(AO_PANIC_MUTEX); + ao_arch_critical( + while (*mutex) + ao_sleep(mutex); + *mutex = ao_cur_task->task_id; + ); +} + +void +ao_mutex_put(__xdata uint8_t *mutex) __reentrant +{ + if (*mutex != ao_cur_task->task_id) + ao_panic(AO_PANIC_MUTEX); + ao_arch_critical( + *mutex = 0; + ao_wakeup(mutex); + ); +} diff --git a/src/kernel/ao_notask.c b/src/kernel/ao_notask.c new file mode 100644 index 00000000..6f967e6d --- /dev/null +++ b/src/kernel/ao_notask.c @@ -0,0 +1,46 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include + +static volatile void *ao_wchan; + +uint8_t +ao_sleep(__xdata void *wchan) +{ +#if 1 + ao_wchan = wchan; + ao_arch_wait_interrupt(); +#else + uint8_t sreg; + + ao_wchan = wchan; + asm("in %0,__SREG__" : "=&r" (sreg)); + sei(); + while (ao_wchan) + ao_arch_cpu_idle(); + asm("out __SREG__,%0" : : "r" (sreg)); +#endif + return 0; +} + +void +ao_wakeup(__xdata void *wchan) +{ + (void) wchan; + ao_wchan = 0; +} diff --git a/src/kernel/ao_notask.h b/src/kernel/ao_notask.h new file mode 100644 index 00000000..6b6b5bb8 --- /dev/null +++ b/src/kernel/ao_notask.h @@ -0,0 +1,27 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_NOTASK_H_ +#define _AO_NOTASK_H_ + +uint8_t +ao_sleep(__xdata void *wchan); + +void +ao_wakeup(__xdata void *wchan); + +#endif /* _AO_NOTASK_H_ */ diff --git a/src/kernel/ao_packet.h b/src/kernel/ao_packet.h new file mode 100644 index 00000000..b8426cf9 --- /dev/null +++ b/src/kernel/ao_packet.h @@ -0,0 +1,91 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PACKET_H_ +#define _AO_PACKET_H_ + +/* + * ao_packet.c + * + * Packet-based command interface + */ + +#define AO_PACKET_MAX 64 +#define AO_PACKET_SYN (uint8_t) 0xff + +struct ao_packet { + uint8_t addr; + uint8_t len; + uint8_t seq; + uint8_t ack; + uint8_t d[AO_PACKET_MAX]; + uint8_t callsign[AO_MAX_CALLSIGN]; +}; + +struct ao_packet_recv { + struct ao_packet packet; + int8_t rssi; + uint8_t status; +}; + +extern __xdata struct ao_packet_recv ao_rx_packet; +extern __xdata struct ao_packet ao_tx_packet; +extern __xdata struct ao_task ao_packet_task; +extern __xdata uint8_t ao_packet_enable; +extern __xdata uint8_t ao_packet_master_sleeping; +extern __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used; +extern __xdata uint8_t ao_packet_restart; + +void +ao_packet_send(void); + +uint8_t +ao_packet_recv(void); + +void +ao_packet_flush(void); + +void +ao_packet_putchar(char c) __reentrant; + +int +_ao_packet_pollchar(void); + +#if PACKET_HAS_MASTER +/* ao_packet_master.c */ + +extern __xdata int8_t ao_packet_last_rssi; + +void +ao_packet_master_init(void); +#endif + +#if PACKET_HAS_SLAVE +/* ao_packet_slave.c */ + +void +ao_packet_slave_start(void); + +void +ao_packet_slave_stop(void); + +void +ao_packet_slave_init(uint8_t enable); + +#endif + +#endif /* _AO_PACKET_H_ */ diff --git a/src/kernel/ao_panic.c b/src/kernel/ao_panic.c new file mode 100644 index 00000000..c29cd8fe --- /dev/null +++ b/src/kernel/ao_panic.c @@ -0,0 +1,90 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +#ifndef HAS_BEEP +#error Please define HAS_BEEP +#endif + +#if !HAS_BEEP +#define ao_beep(x) +#endif +#if !LEDS_AVAILABLE +#define ao_led_on(x) +#define ao_led_off(x) +#endif + +#ifndef AO_LED_PANIC +#define AO_LED_PANIC AO_LED_RED +#endif + +static void +ao_panic_delay(uint8_t n) +{ + uint8_t i = 0, j = 0; + + while (n--) + while (--j) + while (--i) + ao_arch_nop(); +} + +void +ao_panic(uint8_t reason) +{ + uint8_t n; + +#if LOW_LEVEL_DEBUG + ao_cur_task = NULL; + printf ("panic %d\n", reason); +#endif + ao_arch_block_interrupts(); + for (;;) { + ao_panic_delay(20); + for (n = 0; n < 5; n++) { + ao_led_on(AO_LED_PANIC); + ao_beep(AO_BEEP_HIGH); + ao_panic_delay(1); + ao_led_off(AO_LED_PANIC); + ao_beep(AO_BEEP_LOW); + ao_panic_delay(1); + } + ao_beep(AO_BEEP_OFF); + ao_panic_delay(2); + +#ifdef SDCC +#pragma disable_warning 126 +#endif + if (reason & 0x40) { + ao_led_on(AO_LED_PANIC); + ao_beep(AO_BEEP_HIGH); + ao_panic_delay(40); + ao_led_off(AO_LED_PANIC); + ao_beep(AO_BEEP_OFF); + ao_panic_delay(10); + } + for (n = 0; n < (reason & 0x3f); n++) { + ao_led_on(AO_LED_PANIC); + ao_beep(AO_BEEP_MID); + ao_panic_delay(10); + ao_led_off(AO_LED_PANIC); + ao_beep(AO_BEEP_OFF); + ao_panic_delay(10); + } + } +} diff --git a/src/kernel/ao_product.c b/src/kernel/ao_product.c new file mode 100644 index 00000000..b9327bac --- /dev/null +++ b/src/kernel/ao_product.c @@ -0,0 +1,161 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_product.h" + +/* Defines which mark this particular AltOS product */ + +const char ao_version[AO_MAX_VERSION] = AO_iVersion_STRING; +const char ao_manufacturer[] = AO_iManufacturer_STRING; +const char ao_product[] = AO_iProduct_STRING; + +#define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) + +#if HAS_USB + +/* Maximum power in mA */ +#ifndef AO_USB_MAX_POWER +#define AO_USB_MAX_POWER 100 +#endif + +#include "ao_usb.h" +/* USB descriptors in one giant block of bytes */ +AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = +{ + /* Device descriptor */ + 0x12, + AO_USB_DESC_DEVICE, + LE_WORD(0x0110), /* bcdUSB */ + 0x02, /* bDeviceClass */ + 0x00, /* bDeviceSubClass */ + 0x00, /* bDeviceProtocol */ + AO_USB_CONTROL_SIZE, /* bMaxPacketSize */ + LE_WORD(0xFFFE), /* idVendor */ + LE_WORD(AO_idProduct_NUMBER), /* idProduct */ + LE_WORD(0x0100), /* bcdDevice */ + 0x01, /* iManufacturer */ + 0x02, /* iProduct */ + 0x03, /* iSerialNumber */ + 0x01, /* bNumConfigurations */ + + /* Configuration descriptor */ + 0x09, + AO_USB_DESC_CONFIGURATION, + LE_WORD(67), /* wTotalLength */ + 0x02, /* bNumInterfaces */ + 0x01, /* bConfigurationValue */ + 0x00, /* iConfiguration */ + 0xC0, /* bmAttributes */ + AO_USB_MAX_POWER >> 1, /* bMaxPower, 2mA units */ + + /* Control class interface */ + 0x09, + AO_USB_DESC_INTERFACE, + 0x00, /* bInterfaceNumber */ + 0x00, /* bAlternateSetting */ + 0x01, /* bNumEndPoints */ + 0x02, /* bInterfaceClass */ + 0x02, /* bInterfaceSubClass */ + 0x01, /* bInterfaceProtocol, linux requires value of 1 for the cdc_acm module */ + 0x00, /* iInterface */ + + /* Header functional descriptor */ + 0x05, + AO_USB_CS_INTERFACE, + 0x00, /* bDescriptor SubType Header */ + LE_WORD(0x0110), /* CDC version 1.1 */ + + /* Call management functional descriptor */ + 0x05, + AO_USB_CS_INTERFACE, + 0x01, /* bDescriptor SubType Call Management */ + 0x01, /* bmCapabilities = device handles call management */ + 0x01, /* bDataInterface call management interface number */ + + /* ACM functional descriptor */ + 0x04, + AO_USB_CS_INTERFACE, + 0x02, /* bDescriptor SubType Abstract Control Management */ + 0x02, /* bmCapabilities = D1 (Set_line_Coding, Set_Control_Line_State, Get_Line_Coding and Serial_State) */ + + /* Union functional descriptor */ + 0x05, + AO_USB_CS_INTERFACE, + 0x06, /* bDescriptor SubType Union Functional descriptor */ + 0x00, /* bMasterInterface */ + 0x01, /* bSlaveInterface0 */ + + /* Notification EP */ + 0x07, + AO_USB_DESC_ENDPOINT, + AO_USB_INT_EP|0x80, /* bEndpointAddress */ + 0x03, /* bmAttributes = intr */ + LE_WORD(8), /* wMaxPacketSize */ + 0xff, /* bInterval */ + + /* Data class interface descriptor */ + 0x09, + AO_USB_DESC_INTERFACE, + 0x01, /* bInterfaceNumber */ + 0x00, /* bAlternateSetting */ + 0x02, /* bNumEndPoints */ + 0x0A, /* bInterfaceClass = data */ + 0x00, /* bInterfaceSubClass */ + 0x00, /* bInterfaceProtocol */ + 0x00, /* iInterface */ + + /* Data EP OUT */ + 0x07, + AO_USB_DESC_ENDPOINT, + AO_USB_OUT_EP, /* bEndpointAddress */ + 0x02, /* bmAttributes = bulk */ + LE_WORD(AO_USB_OUT_SIZE),/* wMaxPacketSize */ + 0x00, /* bInterval */ + + /* Data EP in */ + 0x07, + AO_USB_DESC_ENDPOINT, + AO_USB_IN_EP|0x80, /* bEndpointAddress */ + 0x02, /* bmAttributes = bulk */ + LE_WORD(AO_USB_IN_SIZE),/* wMaxPacketSize */ + 0x00, /* bInterval */ + + /* String descriptors */ + 0x04, + AO_USB_DESC_STRING, + LE_WORD(0x0409), + + /* iManufacturer */ + AO_iManufacturer_LEN, + AO_USB_DESC_STRING, + AO_iManufacturer_UCS2, + + /* iProduct */ + AO_iProduct_LEN, + AO_USB_DESC_STRING, + AO_iProduct_UCS2, + + /* iSerial */ + AO_iSerial_LEN, + AO_USB_DESC_STRING, + AO_iSerial_UCS2, + + /* Terminating zero */ + 0 +}; +#endif diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c new file mode 100644 index 00000000..e59f5bc4 --- /dev/null +++ b/src/kernel/ao_pyro.c @@ -0,0 +1,518 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include +#include +#include +#endif +#include + +#if IS_COMPANION +#include +#define ao_accel ao_companion_command.accel +#define ao_speed ao_companion_command.speed +#define ao_height ao_companion_command.height +#define ao_flight_state ao_companion_command.flight_state +#define ao_motor_number ao_companion_command.motor_number +#endif + +#define ao_lowbit(x) ((x) & (-x)) + +#ifndef AO_FLIGHT_TEST +enum ao_igniter_status +ao_pyro_status(uint8_t p) +{ + __xdata struct ao_data packet; + __pdata int16_t value; + + ao_arch_critical( + ao_data_get(&packet); + ); + + value = (AO_IGNITER_CLOSED>>1); + value = AO_SENSE_PYRO(&packet, p); + if (value < AO_IGNITER_OPEN) + return ao_igniter_open; + else if (value > AO_IGNITER_CLOSED) + return ao_igniter_ready; + else + return ao_igniter_unknown; +} + +void +ao_pyro_print_status(void) +{ + uint8_t p; + + for(p = 0; p < AO_PYRO_NUM; p++) { + enum ao_igniter_status status = ao_pyro_status(p); + printf("Igniter: %d Status: %s\n", + p, ao_igniter_status_names[status]); + } +} +#endif + +uint16_t ao_pyro_fired; + +/* + * Given a pyro structure, figure out + * if the current flight state satisfies all + * of the requirements + */ +static uint8_t +ao_pyro_ready(struct ao_pyro *pyro) +{ + enum ao_pyro_flag flag, flags; + + flags = pyro->flags; + while (flags != ao_pyro_none) { + flag = ao_lowbit(flags); + flags &= ~flag; + switch (flag) { + + case ao_pyro_accel_less: + if (ao_accel <= pyro->accel_less) + continue; + break; + case ao_pyro_accel_greater: + if (ao_accel >= pyro->accel_greater) + continue; + break; + + + case ao_pyro_speed_less: + if (ao_speed <= pyro->speed_less) + continue; + break; + case ao_pyro_speed_greater: + if (ao_speed >= pyro->speed_greater) + continue; + break; + + case ao_pyro_height_less: + if (ao_height <= pyro->height_less) + continue; + break; + case ao_pyro_height_greater: + if (ao_height >= pyro->height_greater) + continue; + break; + +#if HAS_GYRO + case ao_pyro_orient_less: + if (ao_sample_orient <= pyro->orient_less) + continue; + break; + case ao_pyro_orient_greater: + if (ao_sample_orient >= pyro->orient_greater) + continue; + break; +#endif + + case ao_pyro_time_less: + if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) + continue; + break; + case ao_pyro_time_greater: + if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) + continue; + break; + + case ao_pyro_ascending: + if (ao_speed > 0) + continue; + break; + case ao_pyro_descending: + if (ao_speed < 0) + continue; + break; + + case ao_pyro_after_motor: + if (ao_motor_number == pyro->motor) + continue; + break; + + case ao_pyro_delay: + /* handled separately */ + continue; + + case ao_pyro_state_less: + if (ao_flight_state < pyro->state_less) + continue; + break; + case ao_pyro_state_greater_or_equal: + if (ao_flight_state >= pyro->state_greater_or_equal) + continue; + break; + + default: + continue; + } + return FALSE; + } + return TRUE; +} + +#ifndef AO_FLIGHT_TEST +static void +ao_pyro_pin_set(uint8_t p, uint8_t v) +{ + switch (p) { +#if AO_PYRO_NUM > 0 + case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break; +#endif +#if AO_PYRO_NUM > 1 + case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break; +#endif +#if AO_PYRO_NUM > 2 + case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break; +#endif +#if AO_PYRO_NUM > 3 + case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break; +#endif +#if AO_PYRO_NUM > 4 + case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break; +#endif +#if AO_PYRO_NUM > 5 + case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break; +#endif +#if AO_PYRO_NUM > 6 + case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break; +#endif +#if AO_PYRO_NUM > 7 + case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; +#endif + default: break; + } +} +#endif + +uint8_t ao_pyro_wakeup; + +static void +ao_pyro_pins_fire(uint16_t fire) +{ + uint8_t p; + + for (p = 0; p < AO_PYRO_NUM; p++) { + if (fire & (1 << p)) + ao_pyro_pin_set(p, 1); + } + ao_delay(AO_MS_TO_TICKS(50)); + for (p = 0; p < AO_PYRO_NUM; p++) { + if (fire & (1 << p)) { + ao_pyro_pin_set(p, 0); + ao_config.pyro[p].fired = 1; + ao_pyro_fired |= (1 << p); + } + } + ao_delay(AO_MS_TO_TICKS(50)); +} + +static uint8_t +ao_pyro_check(void) +{ + struct ao_pyro *pyro; + uint8_t p, any_waiting; + uint16_t fire = 0; + + any_waiting = 0; + for (p = 0; p < AO_PYRO_NUM; p++) { + pyro = &ao_config.pyro[p]; + + /* Ignore igniters which have already fired + */ + if (pyro->fired) + continue; + + /* Ignore disabled igniters + */ + if (!pyro->flags) + continue; + + any_waiting = 1; + /* Check pyro state to see if it should fire + */ + if (!pyro->delay_done) { + if (!ao_pyro_ready(pyro)) + continue; + + /* If there's a delay set, then remember when + * it expires + */ + if (pyro->flags & ao_pyro_delay) { + pyro->delay_done = ao_time() + pyro->delay; + if (!pyro->delay_done) + pyro->delay_done = 1; + } + } + + /* Check to see if we're just waiting for + * the delay to expire + */ + if (pyro->delay_done) { + if ((int16_t) (ao_time() - pyro->delay_done) < 0) + continue; + } + + fire |= (1 << p); + } + + if (fire) + ao_pyro_pins_fire(fire); + + return any_waiting; +} + +#define NO_VALUE 0xff + +#define AO_PYRO_NAME_LEN 3 + +#if !DISABLE_HELP +#define ENABLE_HELP 1 +#endif + +#if ENABLE_HELP +#define HELP(s) (s) +#else +#define HELP(s) +#endif + +const struct { + char name[AO_PYRO_NAME_LEN]; + enum ao_pyro_flag flag; + uint8_t offset; +#if ENABLE_HELP + char *help; +#endif +} ao_pyro_values[] = { + { "a<", ao_pyro_accel_less, offsetof(struct ao_pyro, accel_less), HELP("accel less (m/ss * 16)") }, + { "a>", ao_pyro_accel_greater, offsetof(struct ao_pyro, accel_greater), HELP("accel greater (m/ss * 16)") }, + + { "s<", ao_pyro_speed_less, offsetof(struct ao_pyro, speed_less), HELP("speed less (m/s * 16)") }, + { "s>", ao_pyro_speed_greater, offsetof(struct ao_pyro, speed_greater), HELP("speed greater (m/s * 16)") }, + + { "h<", ao_pyro_height_less, offsetof(struct ao_pyro, height_less), HELP("height less (m)") }, + { "h>", ao_pyro_height_greater, offsetof(struct ao_pyro, height_greater), HELP("height greater (m)") }, + +#if HAS_GYRO + { "o<", ao_pyro_orient_less, offsetof(struct ao_pyro, orient_less), HELP("orient less (deg)") }, + { "o>", ao_pyro_orient_greater, offsetof(struct ao_pyro, orient_greater), HELP("orient greater (deg)") }, +#endif + + { "t<", ao_pyro_time_less, offsetof(struct ao_pyro, time_less), HELP("time less (s * 100)") }, + { "t>", ao_pyro_time_greater, offsetof(struct ao_pyro, time_greater), HELP("time greater (s * 100)") }, + + { "f<", ao_pyro_state_less, offsetof(struct ao_pyro, state_less), HELP("state less") }, + { "f>=",ao_pyro_state_greater_or_equal, offsetof(struct ao_pyro, state_greater_or_equal), HELP("state greater or equal") }, + + { "A", ao_pyro_ascending, NO_VALUE, HELP("ascending") }, + { "D", ao_pyro_descending, NO_VALUE, HELP("descending") }, + + { "m", ao_pyro_after_motor, offsetof(struct ao_pyro, motor), HELP("after motor") }, + + { "d", ao_pyro_delay, offsetof(struct ao_pyro, delay), HELP("delay before firing (s * 100)") }, + { "", ao_pyro_none, NO_VALUE, HELP(NULL) }, +}; + +#define NUM_PYRO_VALUES (sizeof ao_pyro_values / sizeof ao_pyro_values[0]) + +#ifndef AO_FLIGHT_TEST +static void +ao_pyro(void) +{ + uint8_t any_waiting; + + ao_config_get(); + while (ao_flight_state < ao_flight_boost) + ao_sleep(&ao_flight_state); + + for (;;) { + ao_alarm(AO_MS_TO_TICKS(100)); + ao_sleep(&ao_pyro_wakeup); + ao_clear_alarm(); + if (ao_flight_state >= ao_flight_landed) + break; + any_waiting = ao_pyro_check(); + if (!any_waiting) + break; + } + ao_exit(); +} + +__xdata struct ao_task ao_pyro_task; + + +static void +ao_pyro_print_name(uint8_t v) +{ + const char *s = ao_pyro_values[v].name; + printf ("%s%s", s, " " + strlen(s)); +} + +#if ENABLE_HELP +static void +ao_pyro_help(void) +{ + uint8_t v; + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + ao_pyro_print_name(v); + if (ao_pyro_values[v].offset != NO_VALUE) + printf (" "); + else + printf (" "); + printf ("%s\n", ao_pyro_values[v].help); + } +} +#endif + +void +ao_pyro_show(void) +{ + uint8_t p; + uint8_t v; + struct ao_pyro *pyro; + + printf ("Pyro-count: %d\n", AO_PYRO_NUM); + for (p = 0; p < AO_PYRO_NUM; p++) { + printf ("Pyro %2d: ", p); + pyro = &ao_config.pyro[p]; + if (!pyro->flags) { + printf ("\n"); + continue; + } + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + if (!(pyro->flags & ao_pyro_values[v].flag)) + continue; + ao_pyro_print_name(v); + if (ao_pyro_values[v].offset != NO_VALUE) { + int16_t value; + + value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); + printf ("%6d ", value); + } else { + printf (" "); + } + } + printf ("\n"); + } +} + +void +ao_pyro_set(void) +{ + uint8_t p; + struct ao_pyro pyro_tmp; + char name[AO_PYRO_NAME_LEN]; + uint8_t c; + uint8_t v; + + ao_cmd_white(); + +#if ENABLE_HELP + switch (ao_cmd_lex_c) { + case '?': + ao_pyro_help(); + return; + } +#endif + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + p = ao_cmd_lex_i; + if (AO_PYRO_NUM <= p) { + printf ("invalid pyro channel %d\n", p); + return; + } + pyro_tmp.flags = 0; + for (;;) { + ao_cmd_white(); + if (ao_cmd_lex_c == '\n') + break; + + for (c = 0; c < AO_PYRO_NAME_LEN - 1; c++) { + if (ao_cmd_is_white()) + break; + name[c] = ao_cmd_lex_c; + ao_cmd_lex(); + } + name[c] = '\0'; + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + if (!strcmp (ao_pyro_values[v].name, name)) + break; + } + if (ao_pyro_values[v].flag == ao_pyro_none) { + printf ("invalid pyro field %s\n", name); + ao_cmd_status = ao_cmd_syntax_error; + return; + } + pyro_tmp.flags |= ao_pyro_values[v].flag; + if (ao_pyro_values[v].offset != NO_VALUE) { + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + } + } + _ao_config_edit_start(); + ao_config.pyro[p] = pyro_tmp; + _ao_config_edit_finish(); +} + +void +ao_pyro_manual(uint8_t p) +{ + printf ("ao_pyro_manual %d\n", p); + if (p >= AO_PYRO_NUM) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + ao_pyro_pins_fire(1 << p); +} + +void +ao_pyro_init(void) +{ +#if AO_PYRO_NUM > 0 + ao_enable_output(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, 0); +#endif +#if AO_PYRO_NUM > 1 + ao_enable_output(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, 0); +#endif +#if AO_PYRO_NUM > 2 + ao_enable_output(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, 0); +#endif +#if AO_PYRO_NUM > 3 + ao_enable_output(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, 0); +#endif +#if AO_PYRO_NUM > 4 + ao_enable_output(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, 0); +#endif +#if AO_PYRO_NUM > 5 + ao_enable_output(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, 0); +#endif +#if AO_PYRO_NUM > 6 + ao_enable_output(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, 0); +#endif +#if AO_PYRO_NUM > 7 + ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); +#endif + ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); +} +#endif diff --git a/src/kernel/ao_pyro.h b/src/kernel/ao_pyro.h new file mode 100644 index 00000000..0c5642d6 --- /dev/null +++ b/src/kernel/ao_pyro.h @@ -0,0 +1,83 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PYRO_H_ +#define _AO_PYRO_H_ + +enum ao_pyro_flag { + ao_pyro_none = 0x00000000, + + ao_pyro_accel_less = 0x00000001, + ao_pyro_accel_greater = 0x00000002, + + ao_pyro_speed_less = 0x00000004, + ao_pyro_speed_greater = 0x00000008, + + ao_pyro_height_less = 0x00000010, + ao_pyro_height_greater = 0x00000020, + + ao_pyro_orient_less = 0x00000040, + ao_pyro_orient_greater = 0x00000080, + + ao_pyro_time_less = 0x00000100, + ao_pyro_time_greater = 0x00000200, + + ao_pyro_ascending = 0x00000400, + ao_pyro_descending = 0x00000800, + + ao_pyro_after_motor = 0x00001000, + + ao_pyro_delay = 0x00002000, + + ao_pyro_state_less = 0x00004000, + ao_pyro_state_greater_or_equal = 0x00008000, +}; + +struct ao_pyro { + enum ao_pyro_flag flags; + int16_t accel_less, accel_greater; + int16_t speed_less, speed_greater; + int16_t height_less, height_greater; + int16_t orient_less, orient_greater; + int16_t time_less, time_greater; + int16_t delay; + uint8_t state_less, state_greater_or_equal; + int16_t motor; + uint16_t delay_done; + uint8_t fired; +}; + +extern uint8_t ao_pyro_wakeup; + +extern uint16_t ao_pyro_fired; + +void +ao_pyro_set(void); + +void +ao_pyro_show(void); + +void +ao_pyro_init(void); + +void +ao_pyro_manual(uint8_t p); + +void +ao_pyro_print_status(void); + +#endif diff --git a/src/kernel/ao_quaternion.h b/src/kernel/ao_quaternion.h new file mode 100644 index 00000000..044f1607 --- /dev/null +++ b/src/kernel/ao_quaternion.h @@ -0,0 +1,249 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_QUATERNION_H_ +#define _AO_QUATERNION_H_ + +#include + +struct ao_quaternion { + float r; /* real bit */ + float x, y, z; /* imaginary bits */ +}; + +static inline void ao_quaternion_multiply(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + struct ao_quaternion t; +#define T(_a,_b) (((a)->_a) * ((b)->_b)) + +/* + * Quaternions + * + * ii = jj = kk = ijk = -1; + * + * kji = 1; + * + * ij = k; ji = -k; + * kj = -i; jk = i; + * ik = -j; ki = j; + * + * Multiplication p * q: + * + * (pr + ipx + jpy + kpz) (qr + iqx + jqy + kqz) = + * + * ( pr * qr + pr * iqx + pr * jqy + pr * kqz) + + * (ipx * qr + ipx * iqx + ipx * jqy + ipx * kqz) + + * (jpy * qr + jpy * iqx + jpy * jqy + jpy * kqz) + + * (kpz * qr + kpz * iqx + kpz * jqy + kpz * kqz) = + * + * + * (pr * qr) + i(pr * qx) + j(pr * qy) + k(pr * qz) + + * i(px * qr) - (px * qx) + k(px * qy) - j(px * qz) + + * j(py * qr) - k(py * qx) - (py * qy) + i(py * qz) + + * k(pz * qr) + j(pz * qx) - i(pz * qy) - (pz * qz) = + * + * 1 * ( (pr * qr) - (px * qx) - (py * qy) - (pz * qz) ) + + * i * ( (pr * qx) + (px * qr) + (py * qz) - (pz * qy) ) + + * j * ( (pr * qy) - (px * qz) + (py * qr) + (pz * qx) ) + + * k * ( (pr * qz) + (px * qy) - (py * qx) + (pz * qr); + */ + + t.r = T(r,r) - T(x,x) - T(y,y) - T(z,z); + t.x = T(r,x) + T(x,r) + T(y,z) - T(z,y); + t.y = T(r,y) - T(x,z) + T(y,r) + T(z,x); + t.z = T(r,z) + T(x,y) - T(y,x) + T(z,r); +#undef T + *r = t; +} + +static inline void ao_quaternion_conjugate(struct ao_quaternion *r, + const struct ao_quaternion *a) +{ + r->r = a->r; + r->x = -a->x; + r->y = -a->y; + r->z = -a->z; +} + +static inline float ao_quaternion_normal(const struct ao_quaternion *a) +{ +#define S(_a) (((a)->_a) * ((a)->_a)) + return S(r) + S(x) + S(y) + S(z); +#undef S +} + +static inline void ao_quaternion_scale(struct ao_quaternion *r, + const struct ao_quaternion *a, + float b) +{ + r->r = a->r * b; + r->x = a->x * b; + r->y = a->y * b; + r->z = a->z * b; +} + +static inline void ao_quaternion_normalize(struct ao_quaternion *r, + const struct ao_quaternion *a) +{ + float n = ao_quaternion_normal(a); + + if (n > 0) + ao_quaternion_scale(r, a, 1/sqrtf(n)); + else + *r = *a; +} + +static inline float ao_quaternion_dot(const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ +#define T(_a) (((a)->_a) * ((b)->_a)) + return T(r) + T(x) + T(y) + T(z); +#undef T +} + + +static inline void ao_quaternion_rotate(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + struct ao_quaternion c; + struct ao_quaternion t; + + ao_quaternion_multiply(&t, b, a); + ao_quaternion_conjugate(&c, b); + ao_quaternion_multiply(r, &t, &c); +} + +/* + * Compute a rotation quaternion between two vectors + * + * cos(θ) + u * sin(θ) + * + * where θ is the angle between the two vectors and u + * is a unit vector axis of rotation + */ + +static inline void ao_quaternion_vectors_to_rotation(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + /* + * The cross product will point orthogonally to the two + * vectors, forming our rotation axis. The length will be + * sin(θ), so these values are already multiplied by that. + */ + + float x = a->y * b->z - a->z * b->y; + float y = a->z * b->x - a->x * b->z; + float z = a->x * b->y - a->y * b->x; + + float s_2 = x*x + y*y + z*z; + float s = sqrtf(s_2); + + /* cos(θ) = a · b / (|a| |b|). + * + * a and b are both unit vectors, so the divisor is one + */ + float c = a->x*b->x + a->y*b->y + a->z*b->z; + + float c_half = sqrtf ((1 + c) / 2); + float s_half = sqrtf ((1 - c) / 2); + + /* + * Divide out the sine factor from the + * cross product, then multiply in the + * half sine factor needed for the quaternion + */ + float s_scale = s_half / s; + + r->x = x * s_scale; + r->y = y * s_scale; + r->z = z * s_scale; + + r->r = c_half; + + ao_quaternion_normalize(r, r); +} + +static inline void ao_quaternion_init_vector(struct ao_quaternion *r, + float x, float y, float z) +{ + r->r = 0; + r->x = x; + r->y = y; + r->z = z; +} + +static inline void ao_quaternion_init_rotation(struct ao_quaternion *r, + float x, float y, float z, + float s, float c) +{ + r->r = c; + r->x = s * x; + r->y = s * y; + r->z = s * z; +} + +static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r) +{ + r->r = 1; + r->x = r->y = r->z = 0; +} + +/* + * The sincosf from newlib just calls sinf and cosf. This is a bit + * faster, if slightly less precise + */ + +static inline void +ao_sincosf(float a, float *s, float *c) { + float _s = sinf(a); + *s = _s; + *c = sqrtf(1 - _s*_s); +} + +/* + * Initialize a quaternion from 1/2 euler rotation angles (in radians). + * + * Yes, it would be nicer if there were a faster way, but because we + * sample the gyros at only 100Hz, we end up getting angles too large + * to take advantage of sin(x) ≃ x. + * + * We might be able to use just a couple of elements of the sin taylor + * series though, instead of the whole sin function? + */ + +static inline void ao_quaternion_init_half_euler(struct ao_quaternion *r, + float x, float y, float z) +{ + float s_x, c_x; + float s_y, c_y; + float s_z, c_z; + + ao_sincosf(x, &s_x, &c_x); + ao_sincosf(y, &s_y, &c_y); + ao_sincosf(z, &s_z, &c_z); + + r->r = c_x * c_y * c_z + s_x * s_y * s_z; + r->x = s_x * c_y * c_z - c_x * s_y * s_z; + r->y = c_x * s_y * c_z + s_x * c_y * s_z; + r->z = c_x * c_y * s_z - s_x * s_y * c_z; +} + +#endif /* _AO_QUATERNION_H_ */ diff --git a/src/kernel/ao_radio_cmac.c b/src/kernel/ao_radio_cmac.c new file mode 100644 index 00000000..bff848f6 --- /dev/null +++ b/src/kernel/ao_radio_cmac.c @@ -0,0 +1,164 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +static __xdata uint8_t ao_radio_cmac_mutex; +__pdata int8_t ao_radio_cmac_rssi; +static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN]; + +static uint8_t +round_len(uint8_t len) +{ + uint8_t rem; + + /* Make sure we transfer at least one packet, and + * then make sure every packet is full. Note that + * there is no length encoded, and that the receiver + * must deal with any extra bytes in the packet + */ + if (len < AO_CMAC_KEY_LEN) + len = AO_CMAC_KEY_LEN; + rem = len % AO_CMAC_KEY_LEN; + if (rem != 0) + len += (AO_CMAC_KEY_LEN - rem); + return len; +} + +/* + * Sign and deliver the data sitting in the cmac buffer + */ +static void +radio_cmac_send(uint8_t len) __reentrant +{ + uint8_t i; + + len = round_len(len); + /* Make sure the AES key is loaded */ + ao_config_get(); + +#if HAS_MONITOR + ao_monitor_set(0); +#endif + + ao_mutex_get(&ao_aes_mutex); + ao_aes_set_mode(ao_aes_mode_cbc_mac); + ao_aes_set_key(ao_config.aes_key); + ao_aes_zero_iv(); + for (i = 0; i < len; i += AO_CMAC_KEY_LEN) { + if (i + AO_CMAC_KEY_LEN < len) + ao_aes_run(&cmac_data[i], NULL); + else + ao_aes_run(&cmac_data[i], &cmac_data[len]); + } + ao_mutex_put(&ao_aes_mutex); + + ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN); +} + +/* + * Receive and validate an incoming packet + */ + +static int8_t +radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant +{ + uint8_t i; + + len = round_len(len); +#if HAS_MONITOR + ao_monitor_set(0); +#endif + i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2, timeout); + + if (!i) { + ao_radio_cmac_rssi = 0; + return AO_RADIO_CMAC_TIMEOUT; + } + + ao_radio_cmac_rssi = ao_radio_rssi; + if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & AO_RADIO_STATUS_CRC_OK)) + return AO_RADIO_CMAC_CRC_ERROR; + + ao_config_get(); + + /* Compute the packet signature + */ + ao_mutex_get(&ao_aes_mutex); + ao_aes_set_mode(ao_aes_mode_cbc_mac); + ao_aes_set_key(ao_config.aes_key); + ao_aes_zero_iv(); + for (i = 0; i < len; i += AO_CMAC_KEY_LEN) { + if (i + AO_CMAC_KEY_LEN < len) + ao_aes_run(&cmac_data[i], NULL); + else + ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]); + } + ao_mutex_put(&ao_aes_mutex); + + /* Check the packet signature against the signature provided + * over the link + */ + + if (memcmp(&cmac_data[len], + &cmac_data[len + AO_CMAC_KEY_LEN + 2], + AO_CMAC_KEY_LEN) != 0) { + return AO_RADIO_CMAC_MAC_ERROR; + } + + return AO_RADIO_CMAC_OK; +} + +int8_t +ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant +{ + if (len > AO_CMAC_MAX_LEN) + return AO_RADIO_CMAC_LEN_ERROR; + ao_mutex_get(&ao_radio_cmac_mutex); + ao_xmemcpy(cmac_data, packet, len); +#if AO_LED_TX + ao_led_on(AO_LED_TX); +#endif + radio_cmac_send(len); +#if AO_LED_TX + ao_led_off(AO_LED_TX); +#endif + ao_mutex_put(&ao_radio_cmac_mutex); + return AO_RADIO_CMAC_OK; +} + +int8_t +ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant +{ + int8_t i; + if (len > AO_CMAC_MAX_LEN) + return AO_RADIO_CMAC_LEN_ERROR; + ao_mutex_get(&ao_radio_cmac_mutex); +#if AO_LED_RX + ao_led_on(AO_LED_RX); +#endif + i = radio_cmac_recv(len, timeout); +#if AO_LED_RX + ao_led_off(AO_LED_RX); +#endif + if (i == AO_RADIO_CMAC_OK) + ao_xmemcpy(packet, cmac_data, len); + ao_mutex_put(&ao_radio_cmac_mutex); + return i; +} + diff --git a/src/kernel/ao_radio_cmac.h b/src/kernel/ao_radio_cmac.h new file mode 100644 index 00000000..e86f31e9 --- /dev/null +++ b/src/kernel/ao_radio_cmac.h @@ -0,0 +1,43 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_RADIO_CMAC_H_ +#define _AO_RADIO_CMAC_H_ + +#include + +#define AO_CMAC_KEY_LEN AO_AES_LEN +#define AO_CMAC_MAX_LEN (128 - AO_CMAC_KEY_LEN) + +extern __pdata int8_t ao_radio_cmac_rssi; + +int8_t +ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant; + +#define AO_RADIO_CMAC_OK 0 +#define AO_RADIO_CMAC_LEN_ERROR -1 +#define AO_RADIO_CMAC_CRC_ERROR -2 +#define AO_RADIO_CMAC_MAC_ERROR -3 +#define AO_RADIO_CMAC_TIMEOUT -4 + +int8_t +ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant; + +void +ao_radio_cmac_init(void); + +#endif /* _AO_RADIO_CMAC_H_ */ diff --git a/src/kernel/ao_radio_cmac_cmd.c b/src/kernel/ao_radio_cmac_cmd.c new file mode 100644 index 00000000..64410921 --- /dev/null +++ b/src/kernel/ao_radio_cmac_cmd.c @@ -0,0 +1,104 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include + +static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN]; + +static uint8_t +getnibble(void) +{ + int8_t b; + + b = ao_cmd_hexchar(getchar()); + if (b < 0) { + ao_cmd_status = ao_cmd_lex_error; + return 0; + } + return (uint8_t) b; +} + +static uint8_t +getbyte(void) +{ + uint8_t b; + b = getnibble() << 4; + b |= getnibble(); + return b; +} + +static void +radio_cmac_send_cmd(void) __reentrant +{ + uint8_t i; + uint8_t len; + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + len = ao_cmd_lex_i; + if (len > AO_CMAC_MAX_LEN) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + flush(); + len = ao_cmd_lex_i; + for (i = 0; i < len; i++) { + cmac_data[i] = getbyte(); + if (ao_cmd_status != ao_cmd_success) + return; + } + ao_radio_cmac_send(cmac_data, len); +} + +static void +radio_cmac_recv_cmd(void) __reentrant +{ + uint8_t len, i; + uint16_t timeout; + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + len = ao_cmd_lex_i; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + timeout = AO_MS_TO_TICKS(ao_cmd_lex_i); + i = ao_radio_cmac_recv(cmac_data, len, timeout); + if (i == AO_RADIO_CMAC_OK) { + printf ("PACKET "); + for (i = 0; i < len; i++) + printf("%02x", cmac_data[i]); + printf (" %d\n", ao_radio_cmac_rssi); + } else + printf ("ERROR %d %d\n", i, ao_radio_cmac_rssi); +} + +static __code struct ao_cmds ao_radio_cmac_cmds[] = { + { radio_cmac_send_cmd, "s \0Send AES-CMAC packet. Bytes to send follow on next line" }, + { radio_cmac_recv_cmd, "S \0Receive AES-CMAC packet. Timeout in ms" }, + { 0, NULL }, +}; + +void +ao_radio_cmac_cmd_init(void) +{ + ao_cmd_register(&ao_radio_cmac_cmds[0]); +} diff --git a/src/kernel/ao_radio_cmac_cmd.h b/src/kernel/ao_radio_cmac_cmd.h new file mode 100644 index 00000000..6b8782de --- /dev/null +++ b/src/kernel/ao_radio_cmac_cmd.h @@ -0,0 +1,24 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_RADIO_CMAC_CMD_H_ +#define _AO_RADIO_CMAC_CMD_H_ + +void +ao_radio_cmac_cmd_init(void); + +#endif /* _AO_RADIO_CMAC_CMD_H_ */ diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c new file mode 100644 index 00000000..1104cd82 --- /dev/null +++ b/src/kernel/ao_report.c @@ -0,0 +1,198 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include +#include + +#define BIT(i,x) ((x) ? (1 << (i)) : 0) +#define MORSE1(a) (1 | BIT(3,a)) +#define MORSE2(a,b) (2 | BIT(3,a) | BIT(4,b)) +#define MORSE3(a,b,c) (3 | BIT(3,a) | BIT(4,b) | BIT(5,c)) +#define MORSE4(a,b,c,d) (4 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d)) +#define MORSE5(a,b,c,d,e) (5 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d) | BIT(7,e)) + +static const uint8_t flight_reports[] = { + MORSE3(0,0,0), /* startup, 'S' */ + MORSE2(0,0), /* idle 'I' */ + MORSE4(0,1,1,0), /* pad 'P' */ + MORSE4(1,0,0,0), /* boost 'B' */ + MORSE4(0,0,1,0), /* fast 'F' */ + MORSE4(1,0,1,0), /* coast 'C' */ + MORSE3(1,0,0), /* drogue 'D' */ + MORSE2(1,1), /* main 'M' */ + MORSE4(0,1,0,0), /* landed 'L' */ + MORSE4(1,0,0,1), /* invalid 'X' */ +}; + +#if HAS_BEEP +#define low(time) ao_beep_for(AO_BEEP_LOW, time) +#define mid(time) ao_beep_for(AO_BEEP_MID, time) +#define high(time) ao_beep_for(AO_BEEP_HIGH, time) +#else +#define low(time) ao_led_for(AO_LED_GREEN, time) +#define mid(time) ao_led_for(AO_LED_RED, time) +#define high(time) ao_led_for(AO_LED_GREEN|AO_LED_RED, time) +#endif +#define pause(time) ao_delay(time) + +static __pdata enum ao_flight_state ao_report_state; + +static void +ao_report_beep(void) __reentrant +{ + uint8_t r = flight_reports[ao_flight_state]; + uint8_t l = r & 7; + + if (!r) + return; + while (l--) { + if (r & 8) + mid(AO_MS_TO_TICKS(600)); + else + mid(AO_MS_TO_TICKS(200)); + pause(AO_MS_TO_TICKS(200)); + r >>= 1; + } + pause(AO_MS_TO_TICKS(400)); +} + +static void +ao_report_digit(uint8_t digit) __reentrant +{ + if (!digit) { + mid(AO_MS_TO_TICKS(500)); + pause(AO_MS_TO_TICKS(200)); + } else { + while (digit--) { + mid(AO_MS_TO_TICKS(200)); + pause(AO_MS_TO_TICKS(200)); + } + } + pause(AO_MS_TO_TICKS(300)); +} + +static void +ao_report_altitude(void) +{ + __pdata int16_t agl = ao_max_height; + __xdata uint8_t digits[10]; + __pdata uint8_t ndigits, i; + + if (agl < 0) + agl = 0; + ndigits = 0; + do { + digits[ndigits++] = agl % 10; + agl /= 10; + } while (agl); + + i = ndigits; + do + ao_report_digit(digits[--i]); + while (i != 0); +} + +#if HAS_IGNITE_REPORT +static uint8_t +ao_report_igniter_ready(enum ao_igniter igniter) +{ + return ao_igniter_status(igniter) == ao_igniter_ready ? 1 : 0; +} + +uint8_t +ao_report_igniter(void) +{ + return (ao_report_igniter_ready(ao_igniter_drogue) | + (ao_report_igniter_ready(ao_igniter_main) << 1)); +} + +static void +ao_report_continuity(void) __reentrant +{ + uint8_t c; + +#if !HAS_IGNITE + if (!ao_igniter_present) + return; +#endif + c = ao_report_igniter(); + if (c) { + while (c--) { + high(AO_MS_TO_TICKS(25)); + pause(AO_MS_TO_TICKS(100)); + } + } else { + c = 10; + while (c--) { + high(AO_MS_TO_TICKS(20)); + low(AO_MS_TO_TICKS(20)); + } + } +#if HAS_LOG + if (ao_log_full()) { + pause(AO_MS_TO_TICKS(100)); + c = 2; + while (c--) { + low(AO_MS_TO_TICKS(100)); + mid(AO_MS_TO_TICKS(100)); + high(AO_MS_TO_TICKS(100)); + mid(AO_MS_TO_TICKS(100)); + } + } +#endif +} +#endif + +void +ao_report(void) +{ + ao_report_state = ao_flight_state; + for(;;) { + ao_report_beep(); + if (ao_flight_state == ao_flight_landed) { + ao_report_altitude(); +#if HAS_FLIGHT + ao_delay(AO_SEC_TO_TICKS(5)); + continue; +#endif + } +#if HAS_IGNITE_REPORT + if (ao_flight_state == ao_flight_idle) + ao_report_continuity(); + while (ao_flight_state == ao_flight_pad) { + uint8_t c; + ao_report_continuity(); + c = 50; + while (c-- && ao_flight_state == ao_flight_pad) + pause(AO_MS_TO_TICKS(100)); + } +#endif + + while (ao_report_state == ao_flight_state) + ao_sleep(DATA_TO_XDATA(&ao_flight_state)); + ao_report_state = ao_flight_state; + } +} + +static __xdata struct ao_task ao_report_task; + +void +ao_report_init(void) +{ + ao_add_task(&ao_report_task, ao_report, "report"); +} diff --git a/src/kernel/ao_report_micro.c b/src/kernel/ao_report_micro.c new file mode 100644 index 00000000..0e8e287f --- /dev/null +++ b/src/kernel/ao_report_micro.c @@ -0,0 +1,57 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include + +#define mid(time) ao_led_for(AO_LED_REPORT, time) +#define pause(time) ao_delay(time) + +static void +ao_report_digit(uint8_t digit) __reentrant +{ + if (!digit) { + mid(AO_MS_TO_TICKS(1000)); + pause(AO_MS_TO_TICKS(300)); + } else { + while (digit--) { + mid(AO_MS_TO_TICKS(300)); + pause(AO_MS_TO_TICKS(300)); + } + } + pause(AO_MS_TO_TICKS(1000)); +} + +void +ao_report_altitude(void) +{ + __pdata alt_t agl = ao_max_height; + static __xdata uint8_t digits[11]; + __pdata uint8_t ndigits, i; + + if (agl < 0) + agl = 0; + ndigits = 0; + do { + digits[ndigits++] = agl % 10; + agl /= 10; + } while (agl); + + i = ndigits; + do + ao_report_digit(digits[--i]); + while (i != 0); +} diff --git a/src/kernel/ao_rssi.c b/src/kernel/ao_rssi.c new file mode 100644 index 00000000..244a84fe --- /dev/null +++ b/src/kernel/ao_rssi.c @@ -0,0 +1,53 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +static __xdata uint16_t ao_rssi_time; +static __pdata uint16_t ao_rssi_delay; +static __pdata uint8_t ao_rssi_led; + +void +ao_rssi(void) +{ + for (;;) { + while ((int16_t) (ao_time() - ao_rssi_time) > AO_SEC_TO_TICKS(3)) + ao_sleep(&ao_rssi_time); + ao_led_for(ao_rssi_led, AO_MS_TO_TICKS(100)); + ao_delay(ao_rssi_delay); + } +} + +void +ao_rssi_set(int rssi_value) +{ + if (rssi_value > 0) + rssi_value = 0; + ao_rssi_delay = AO_MS_TO_TICKS((-rssi_value) * 5); + ao_rssi_time = ao_time(); + ao_wakeup(&ao_rssi_time); +} + +__xdata struct ao_task ao_rssi_task; + +void +ao_rssi_init(uint8_t rssi_led) +{ + ao_rssi_led = rssi_led; + ao_rssi_delay = 0; + ao_add_task(&ao_rssi_task, ao_rssi, "rssi"); +} diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c new file mode 100644 index 00000000..34658951 --- /dev/null +++ b/src/kernel/ao_sample.c @@ -0,0 +1,372 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#include +#endif + +#if HAS_GYRO +#include +#endif + +/* + * Current sensor values + */ + +#ifndef PRES_TYPE +#define PRES_TYPE int32_t +#define ALT_TYPE int32_t +#define ACCEL_TYPE int16_t +#endif + +__pdata uint16_t ao_sample_tick; /* time of last data */ +__pdata pres_t ao_sample_pres; +__pdata alt_t ao_sample_alt; +__pdata alt_t ao_sample_height; +#if HAS_ACCEL +__pdata accel_t ao_sample_accel; +#endif +#if HAS_GYRO +__pdata accel_t ao_sample_accel_along; +__pdata accel_t ao_sample_accel_across; +__pdata accel_t ao_sample_accel_through; +__pdata gyro_t ao_sample_roll; +__pdata gyro_t ao_sample_pitch; +__pdata gyro_t ao_sample_yaw; +__pdata angle_t ao_sample_orient; +#endif + +__data uint8_t ao_sample_data; + +/* + * Sensor calibration values + */ + +__pdata pres_t ao_ground_pres; /* startup pressure */ +__pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ + +#if HAS_ACCEL +__pdata accel_t ao_ground_accel; /* startup acceleration */ +__pdata accel_t ao_accel_2g; /* factory accel calibration */ +__pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ +#endif + +#if HAS_GYRO +__pdata accel_t ao_ground_accel_along; +__pdata accel_t ao_ground_accel_across; +__pdata accel_t ao_ground_accel_through; +__pdata int32_t ao_ground_pitch; +__pdata int32_t ao_ground_yaw; +__pdata int32_t ao_ground_roll; +#endif + +static __pdata uint8_t ao_preflight; /* in preflight mode */ + +static __pdata uint16_t nsamples; +__pdata int32_t ao_sample_pres_sum; +#if HAS_ACCEL +__pdata int32_t ao_sample_accel_sum; +#endif +#if HAS_GYRO +__pdata int32_t ao_sample_accel_along_sum; +__pdata int32_t ao_sample_accel_across_sum; +__pdata int32_t ao_sample_accel_through_sum; +__pdata int32_t ao_sample_pitch_sum; +__pdata int32_t ao_sample_yaw_sum; +__pdata int32_t ao_sample_roll_sum; +static struct ao_quaternion ao_rotation; +#endif + +#if HAS_FLIGHT_DEBUG +extern uint8_t ao_orient_test; +#endif + +static void +ao_sample_preflight_add(void) +{ +#if HAS_ACCEL + ao_sample_accel_sum += ao_sample_accel; +#endif + ao_sample_pres_sum += ao_sample_pres; +#if HAS_GYRO + ao_sample_accel_along_sum += ao_sample_accel_along; + ao_sample_accel_across_sum += ao_sample_accel_across; + ao_sample_accel_through_sum += ao_sample_accel_through; + ao_sample_pitch_sum += ao_sample_pitch; + ao_sample_yaw_sum += ao_sample_yaw; + ao_sample_roll_sum += ao_sample_roll; +#endif + ++nsamples; +} + +static void +ao_sample_preflight_set(void) +{ +#if HAS_ACCEL + ao_ground_accel = ao_sample_accel_sum >> 9; + ao_sample_accel_sum = 0; +#endif + ao_ground_pres = ao_sample_pres_sum >> 9; + ao_ground_height = pres_to_altitude(ao_ground_pres); + ao_sample_pres_sum = 0; +#if HAS_GYRO + ao_ground_accel_along = ao_sample_accel_along_sum >> 9; + ao_ground_accel_across = ao_sample_accel_across_sum >> 9; + ao_ground_accel_through = ao_sample_accel_through_sum >> 9; + ao_ground_pitch = ao_sample_pitch_sum; + ao_ground_yaw = ao_sample_yaw_sum; + ao_ground_roll = ao_sample_roll_sum; + ao_sample_accel_along_sum = 0; + ao_sample_accel_across_sum = 0; + ao_sample_accel_through_sum = 0; + ao_sample_pitch_sum = 0; + ao_sample_yaw_sum = 0; + ao_sample_roll_sum = 0; + ao_sample_orient = 0; + + struct ao_quaternion orient; + + /* Take the pad IMU acceleration values and compute our current direction + */ + + ao_quaternion_init_vector(&orient, + (ao_ground_accel_across - ao_config.accel_zero_across), + (ao_ground_accel_through - ao_config.accel_zero_through), + (ao_ground_accel_along - ao_config.accel_zero_along)); + + ao_quaternion_normalize(&orient, + &orient); + + /* Here's up */ + + struct ao_quaternion up = { .r = 0, .x = 0, .y = 0, .z = 1 }; + + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + up.z = -1; + + /* Compute rotation to get from up to our current orientation, set + * that as the current rotation vector + */ + ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient); +#if HAS_FLIGHT_DEBUG + if (ao_orient_test) + printf("\n\treset\n"); +#endif +#endif + nsamples = 0; +} + +#if HAS_GYRO + +#define TIME_DIV 200.0f + +static void +ao_sample_rotate(void) +{ +#ifdef AO_FLIGHT_TEST + float dt = (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; +#else + static const float dt = 1/TIME_DIV; +#endif + float x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt; + float y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt; + float z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt; + struct ao_quaternion rot; + + ao_quaternion_init_half_euler(&rot, x, y, z); + ao_quaternion_multiply(&ao_rotation, &rot, &ao_rotation); + + /* And normalize to make sure it remains a unit vector */ + ao_quaternion_normalize(&ao_rotation, &ao_rotation); + + /* Compute pitch angle from vertical by taking the pad + * orientation vector and rotating it by the current total + * rotation value. That will be a unit vector pointing along + * the airframe axis. The Z value will be the cosine of the + * change in the angle from vertical since boost. + * + * rot = ao_rotation * vertical * ao_rotation° + * rot = ao_rotation * (0,0,0,1) * ao_rotation° + * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) + * = a.z² - a.y² - a.x² + a.r² + * + * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° + * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) + * = -a.z² + a.y² + a.x² - a.r² + */ + + float rotz; + rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r; + + ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI); + +#if HAS_FLIGHT_DEBUG + if (ao_orient_test) { + printf ("rot %d %d %d orient %d \r", + (int) (x * 1000), + (int) (y * 1000), + (int) (z * 1000), + ao_sample_orient); + } +#endif + +} +#endif + +static void +ao_sample_preflight(void) +{ + /* startup state: + * + * Collect 512 samples of acceleration and pressure + * data and average them to find the resting values + */ + if (nsamples < 512) { + ao_sample_preflight_add(); + } else { +#if HAS_ACCEL + ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; + ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g; +#endif + ao_sample_preflight_set(); + ao_preflight = FALSE; + } +} + +/* + * While in pad mode, constantly update the ground state by + * re-averaging the data. This tracks changes in orientation, which + * might be caused by adjustments to the rocket on the pad and + * pressure, which might be caused by changes in the weather. + */ + +static void +ao_sample_preflight_update(void) +{ + if (nsamples < 512) + ao_sample_preflight_add(); + else if (nsamples < 1024) + ++nsamples; + else + ao_sample_preflight_set(); +} + +#if 0 +#if HAS_GYRO +static int32_t p_filt; +static int32_t y_filt; + +static gyro_t inline ao_gyro(void) { + gyro_t p = ao_sample_pitch - ao_ground_pitch; + gyro_t y = ao_sample_yaw - ao_ground_yaw; + + p_filt = p_filt - (p_filt >> 6) + p; + y_filt = y_filt - (y_filt >> 6) + y; + + p = p_filt >> 6; + y = y_filt >> 6; + return ao_sqrt(p*p + y*y); +} +#endif +#endif + +uint8_t +ao_sample(void) +{ + ao_wakeup(DATA_TO_XDATA(&ao_sample_data)); + ao_sleep((void *) DATA_TO_XDATA(&ao_data_head)); + while (ao_sample_data != ao_data_head) { + __xdata struct ao_data *ao_data; + + /* Capture a sample */ + ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data]; + ao_sample_tick = ao_data->tick; + +#if HAS_BARO + ao_data_pres_cook(ao_data); + ao_sample_pres = ao_data_pres(ao_data); + ao_sample_alt = pres_to_altitude(ao_sample_pres); + ao_sample_height = ao_sample_alt - ao_ground_height; +#endif + +#if HAS_ACCEL + ao_sample_accel = ao_data_accel_cook(ao_data); + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + ao_sample_accel = ao_data_accel_invert(ao_sample_accel); + ao_data_set_accel(ao_data, ao_sample_accel); +#endif +#if HAS_GYRO + ao_sample_accel_along = ao_data_along(ao_data); + ao_sample_accel_across = ao_data_across(ao_data); + ao_sample_accel_through = ao_data_through(ao_data); + ao_sample_pitch = ao_data_pitch(ao_data); + ao_sample_yaw = ao_data_yaw(ao_data); + ao_sample_roll = ao_data_roll(ao_data); +#endif + + if (ao_preflight) + ao_sample_preflight(); + else { + if (ao_flight_state < ao_flight_boost) + ao_sample_preflight_update(); + ao_kalman(); +#if HAS_GYRO + ao_sample_rotate(); +#endif + } +#ifdef AO_FLIGHT_TEST + ao_sample_prev_tick = ao_sample_tick; +#endif + ao_sample_data = ao_data_ring_next(ao_sample_data); + } + return !ao_preflight; +} + +void +ao_sample_init(void) +{ + ao_config_get(); + nsamples = 0; + ao_sample_pres_sum = 0; + ao_sample_pres = 0; +#if HAS_ACCEL + ao_sample_accel_sum = 0; + ao_sample_accel = 0; +#endif +#if HAS_GYRO + ao_sample_accel_along_sum = 0; + ao_sample_accel_across_sum = 0; + ao_sample_accel_through_sum = 0; + ao_sample_accel_along = 0; + ao_sample_accel_across = 0; + ao_sample_accel_through = 0; + ao_sample_pitch_sum = 0; + ao_sample_yaw_sum = 0; + ao_sample_roll_sum = 0; + ao_sample_pitch = 0; + ao_sample_yaw = 0; + ao_sample_roll = 0; + ao_sample_orient = 0; +#endif + ao_sample_data = ao_data_head; + ao_preflight = TRUE; +} diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h new file mode 100644 index 00000000..16d4c507 --- /dev/null +++ b/src/kernel/ao_sample.h @@ -0,0 +1,156 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_SAMPLE_H_ +#define _AO_SAMPLE_H_ + +#include + +/* + * ao_sample.c + */ + +/* + * Barometer calibration + * + * We directly sample the barometer. The specs say: + * + * Pressure range: 15-115 kPa + * Voltage at 115kPa: 2.82 + * Output scale: 27mV/kPa + * + * If we want to detect launch with the barometer, we need + * a large enough bump to not be fooled by noise. At typical + * launch elevations (0-2000m), a 200Pa pressure change cooresponds + * to about a 20m elevation change. This is 5.4mV, or about 3LSB. + * As all of our calculations are done in 16 bits, we'll actually see a change + * of 16 times this though + * + * 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa + */ + +/* Accelerometer calibration + * + * We're sampling the accelerometer through a resistor divider which + * consists of 5k and 10k resistors. This multiplies the values by 2/3. + * That goes into the cc1111 A/D converter, which is running at 11 bits + * of precision with the bits in the MSB of the 16 bit value. Only positive + * values are used, so values should range from 0-32752 for 0-3.3V. The + * specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what + * the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV, + * for a final computation of: + * + * 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g + * + * Zero g was measured at 16000 (we would expect 16384). + * Note that this value is only require to tell if the + * rocket is standing upright. Once that is determined, + * the value of the accelerometer is averaged for 100 samples + * to find the resting accelerometer value, which is used + * for all further flight computations + */ + +/* + * Above this height, the baro sensor doesn't work + */ +#if HAS_MS5607 +#define AO_MAX_BARO_HEIGHT 30000 +#else +#define AO_MAX_BARO_HEIGHT 12000 +#endif + +/* + * Above this speed, baro measurements are unreliable + */ +#define AO_MAX_BARO_SPEED 200 + +#define ACCEL_NOSE_UP (ao_accel_2g >> 2) + +/* + * Speed and acceleration are scaled by 16 to provide a bit more + * resolution while still having reasonable range. Note that this + * limits speed to 2047m/s (around mach 6) and acceleration to + * 2047m/s² (over 200g) + */ + +#define AO_M_TO_HEIGHT(m) ((int16_t) (m)) +#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) +#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) + +extern __pdata uint16_t ao_sample_tick; /* time of last data */ +extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */ +extern __data uint8_t ao_sample_data; /* Ring position of last processed sample */ + +#if HAS_BARO +extern __pdata pres_t ao_sample_pres; /* most recent pressure sensor reading */ +extern __pdata alt_t ao_sample_alt; /* MSL of ao_sample_pres */ +extern __pdata alt_t ao_sample_height; /* AGL of ao_sample_pres */ +extern __pdata pres_t ao_ground_pres; /* startup pressure */ +extern __pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ +#endif + +#if HAS_ACCEL +extern __pdata accel_t ao_sample_accel; /* most recent accel sensor reading */ +extern __pdata accel_t ao_ground_accel; /* startup acceleration */ +extern __pdata accel_t ao_accel_2g; /* factory accel calibration */ +extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ +#endif +#if HAS_GYRO +extern __pdata accel_t ao_ground_accel_along; +extern __pdata accel_t ao_ground_accel_across; +extern __pdata accel_t ao_ground_accel_through; +extern __pdata int32_t ao_ground_pitch; /* * 512 */ +extern __pdata int32_t ao_ground_yaw; /* * 512 */ +extern __pdata int32_t ao_ground_roll; /* * 512 */ +extern __pdata accel_t ao_sample_accel_along; +extern __pdata accel_t ao_sample_accel_across; +extern __pdata accel_t ao_sample_accel_through; +extern __pdata gyro_t ao_sample_roll; +extern __pdata gyro_t ao_sample_pitch; +extern __pdata gyro_t ao_sample_yaw; +extern __pdata angle_t ao_sample_orient; +#endif + +void ao_sample_init(void); + +/* returns FALSE in preflight mode, TRUE in flight mode */ +uint8_t ao_sample(void); + +/* + * ao_kalman.c + */ + +#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define from_fix(x) ((x) >> 16) + +extern __pdata int16_t ao_height; /* meters */ +extern __pdata int16_t ao_speed; /* m/s * 16 */ +extern __pdata int16_t ao_accel; /* m/s² * 16 */ +extern __xdata int16_t ao_max_height; /* max of ao_height */ +extern __xdata int16_t ao_avg_height; /* running average of height */ + +extern __pdata int16_t ao_error_h; +extern __pdata int16_t ao_error_h_sq_avg; + +#if HAS_ACCEL +extern __pdata int16_t ao_error_a; +#endif + +void ao_kalman(void); + +#endif /* _AO_SAMPLE_H_ */ diff --git a/src/kernel/ao_sample_profile.c b/src/kernel/ao_sample_profile.c new file mode 100644 index 00000000..d3743d12 --- /dev/null +++ b/src/kernel/ao_sample_profile.c @@ -0,0 +1,173 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include + +#ifndef AO_SAMPLE_PROFILE_LOW_PC +#define AO_SAMPLE_PROFILE_LOW_PC 0x08002000 +#endif + +#ifndef AO_SAMPLE_PROFILE_HIGH_PC +#define AO_SAMPLE_PROFILE_HIGH_PC 0x0800f000 +#endif + +#ifndef AO_SAMPLE_PROFILE_SHIFT +#define AO_SAMPLE_PROFILE_SHIFT 6 +#endif + +#define AO_SAMPLE_PROFILE_RANGE (AO_SAMPLE_PROFILE_HIGH_PC - AO_SAMPLE_PROFILE_LOW_PC) +#define AO_SAMPLE_PROFILE_NUM (AO_SAMPLE_PROFILE_RANGE >> AO_SAMPLE_PROFILE_SHIFT) + +static uint16_t prev_tick; +static uint16_t samples[AO_SAMPLE_PROFILE_NUM]; +static uint8_t missed[AO_SAMPLE_PROFILE_NUM/8]; +static uint16_t max_miss; +static uint32_t task, isr, os, idle; + +extern uint8_t ao_idle_loc; + +void +ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr) +{ + uint16_t delta = tick - prev_tick; + + if (pc < AO_SAMPLE_PROFILE_LOW_PC) + return; + if (pc >= AO_SAMPLE_PROFILE_HIGH_PC) + return; + if (ao_cur_task) { + uint8_t *sp; + int32_t sp_delta; + + asm("mov %0,sp" : "=&r" (sp)); + sp_delta = sp - (uint8_t *) ao_cur_task->stack; + if (-96 < sp_delta && sp_delta < 16) + ao_panic(AO_PANIC_STACK); + } + + if (in_isr) + isr += delta; + else if (ao_cur_task) { + ao_cur_task->ticks += delta; + task += delta; + } else if (pc == (uint32_t) &ao_idle_loc) + idle += delta; + else + os += delta; + + pc -= AO_SAMPLE_PROFILE_LOW_PC; + pc >>= AO_SAMPLE_PROFILE_SHIFT; + samples[pc] += delta; + + if (delta > 1) + missed[pc >> 3] |= (1 << (pc & 7)); + if (delta > max_miss) + max_miss = delta; + prev_tick = tick; +} + +static void +ao_sample_profile_start(void) +{ + prev_tick = ao_sample_profile_timer_start(); +} + +static void +ao_sample_profile_stop(void) +{ + ao_sample_profile_timer_stop(); +} + +static void +ao_sample_profile_dump(void) +{ + uint16_t a; + uint8_t t; + + printf ("task %6d\n", task); + printf ("isr %6d\n", isr); + printf ("os %6d\n", os); + printf ("idle %6d\n", idle); + printf ("irq blocked %d\n", max_miss); + for (t = 0; t < ao_num_tasks; t++) + printf ("task %6d %6d %6d %s\n", + ao_tasks[t]->ticks, + ao_tasks[t]->yields, + ao_tasks[t]->max_run, + ao_tasks[t]->name); + for (a = 0; a < AO_SAMPLE_PROFILE_NUM; a++) { + if (samples[a]) + printf ("%04x %c %u\n", + (a << AO_SAMPLE_PROFILE_SHIFT) + AO_SAMPLE_PROFILE_LOW_PC, + missed[a >> 3] & (1 << (a & 7)) ? '*' : ' ', + samples[a]); + } +} + +static void +ao_sample_profile_clear(void) +{ + int t; + + task = isr = os = idle = 0; + max_miss = 0; + memset(samples, '\0', sizeof (samples)); + memset(missed, '\0', sizeof (missed)); + for (t = 0; t < ao_num_tasks; t++) { + ao_tasks[t]->ticks = 0; + ao_tasks[t]->yields = 0; + ao_tasks[t]->max_run = 0; + } +} + +static void +ao_sample_profile_cmd(void) +{ + ao_cmd_white(); + switch (ao_cmd_lex_c) { + case '1': + ao_sample_profile_start(); + break; + case '0': + ao_sample_profile_stop(); + break; + case 'd': + ao_sample_profile_dump(); + break; + case 'c': + ao_sample_profile_clear(); + break; + default: + ao_cmd_status = ao_cmd_syntax_error; + break; + } +} + +static __code struct ao_cmds ao_sample_profile_cmds[] = { + { ao_sample_profile_cmd, "S <1 start,0 stop, d dump,c clear>\0Sample profile" }, + { 0, NULL } +}; + +void +ao_sample_profile_init(void) +{ + ao_sample_profile_timer_init(); + ao_cmd_register(&ao_sample_profile_cmds[0]); + ao_sample_profile_clear(); +} diff --git a/src/kernel/ao_sample_profile.h b/src/kernel/ao_sample_profile.h new file mode 100644 index 00000000..dbc29d3d --- /dev/null +++ b/src/kernel/ao_sample_profile.h @@ -0,0 +1,29 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_SAMPLE_PROFILE_H_ +#define _AO_SAMPLE_PROFILE_H_ + +#include + +void +ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr); + +void +ao_sample_profile_init(void); + +#endif /* _AO_SAMPLE_PROFILE_H_ */ diff --git a/src/kernel/ao_send_packet.c b/src/kernel/ao_send_packet.c new file mode 100644 index 00000000..66315d22 --- /dev/null +++ b/src/kernel/ao_send_packet.c @@ -0,0 +1,58 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +#define AO_MAX_SEND 128 + +static __xdata uint8_t ao_send[AO_MAX_SEND]; + +static void +ao_send_packet(void) +{ + __pdata uint16_t count; + uint8_t b; + __pdata uint8_t i; + + ao_cmd_hex(); + count = ao_cmd_lex_i; + if (ao_cmd_status != ao_cmd_success) + return; + if (count > AO_MAX_SEND - 2) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + for (i = 0; i < count; i++) { + b = ao_getnibble() << 4; + b |= ao_getnibble(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_send[i] = b; + } + ao_radio_send(ao_send, count); +} + +static __code struct ao_cmds ao_send_packet_cmds[] = { + { ao_send_packet, "S \0Send packet. Data on next line" }, + { 0, NULL } +}; + +void +ao_send_packet_init(void) +{ + ao_cmd_register(&ao_send_packet_cmds[0]); +} diff --git a/src/kernel/ao_send_packet.h b/src/kernel/ao_send_packet.h new file mode 100644 index 00000000..526f7b55 --- /dev/null +++ b/src/kernel/ao_send_packet.h @@ -0,0 +1,24 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_SEND_PACKET_H_ +#define _AO_SEND_PACKET_H_ + +void +ao_send_packet_init(void); + +#endif /* _AO_SEND_PACKET_H_ */ diff --git a/src/kernel/ao_serial.h b/src/kernel/ao_serial.h new file mode 100644 index 00000000..baf213c0 --- /dev/null +++ b/src/kernel/ao_serial.h @@ -0,0 +1,110 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_SERIAL_H_ +#define _AO_SERIAL_H_ + +#define AO_SERIAL_SPEED_4800 0 +#define AO_SERIAL_SPEED_9600 1 +#define AO_SERIAL_SPEED_19200 2 +#define AO_SERIAL_SPEED_57600 3 +#define AO_SERIAL_SPEED_115200 4 + +#if HAS_SERIAL_0 +extern volatile __xdata struct ao_fifo ao_serial0_rx_fifo; +extern volatile __xdata struct ao_fifo ao_serial0_tx_fifo; + +char +ao_serial0_getchar(void); + +int +_ao_serial0_pollchar(void); + +void +ao_serial0_putchar(char c); + +void +ao_serial0_drain(void); + +void +ao_serial0_set_speed(uint8_t speed); +#endif + +#if HAS_SERIAL_1 +extern volatile __xdata struct ao_fifo ao_serial1_rx_fifo; +extern volatile __xdata struct ao_fifo ao_serial1_tx_fifo; + +char +ao_serial1_getchar(void); + +int +_ao_serial1_pollchar(void); + +void +ao_serial1_putchar(char c); + +void +ao_serial1_drain(void); + +void +ao_serial1_set_speed(uint8_t speed); +#endif + +#if HAS_SERIAL_2 +extern volatile __xdata struct ao_fifo ao_serial2_rx_fifo; +extern volatile __xdata struct ao_fifo ao_serial2_tx_fifo; + +char +ao_serial2_getchar(void); + +int +_ao_serial2_pollchar(void); + +void +ao_serial2_putchar(char c); + +void +ao_serial2_drain(void); + +void +ao_serial2_set_speed(uint8_t speed); +#endif + +#if HAS_SERIAL_3 +extern volatile __xdata struct ao_fifo ao_serial3_rx_fifo; +extern volatile __xdata struct ao_fifo ao_serial3_tx_fifo; + +char +ao_serial3_getchar(void); + +int +_ao_serial3_pollchar(void); + +void +ao_serial3_putchar(char c); + +void +ao_serial3_drain(void); + +void +ao_serial3_set_speed(uint8_t speed); +#endif + +void +ao_serial_init(void); + +#endif /* _AO_SERIAL_H_ */ diff --git a/src/kernel/ao_sqrt.c b/src/kernel/ao_sqrt.c new file mode 100644 index 00000000..3a550eaa --- /dev/null +++ b/src/kernel/ao_sqrt.c @@ -0,0 +1,48 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#endif + +/* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */ +/** + * int_sqrt - rough approximation to sqrt + * @x: integer of which to calculate the sqrt + * + * A very rough approximation to the sqrt() function. + */ + +uint32_t +ao_sqrt(uint32_t op) +{ + uint32_t res = 0; + uint32_t one = 1UL << (sizeof (one) * 8 - 2); + + while (one > op) + one >>= 2; + + while (one != 0) { + if (op >= res + one) { + op = op - (res + one); + res = res + 2 * one; + } + res /= 2; + one /= 4; + } + return res; +} diff --git a/src/kernel/ao_state.c b/src/kernel/ao_state.c new file mode 100644 index 00000000..ed197aa5 --- /dev/null +++ b/src/kernel/ao_state.c @@ -0,0 +1,23 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +const char const * const ao_state_names[] = { + "startup", "idle", "pad", "boost", "fast", + "coast", "drogue", "main", "landed", "invalid" +}; diff --git a/src/kernel/ao_stdio.c b/src/kernel/ao_stdio.c new file mode 100644 index 00000000..99118137 --- /dev/null +++ b/src/kernel/ao_stdio.c @@ -0,0 +1,158 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +/* + * Basic I/O functions to support SDCC stdio package + */ + +#ifndef USE_SERIAL_0_STDIN +#define USE_SERIAL_0_STDIN 0 +#endif +#ifndef USE_SERIAL_1_STDIN +#define USE_SERIAL_1_STDIN 0 +#endif +#ifndef USE_SERIAL_2_STDIN +#define USE_SERIAL_2_STDIN 0 +#endif +#ifndef USE_SERIAL_3_STDIN +#define USE_SERIAL_3_STDIN 0 +#endif +#ifndef USE_SERIAL_4_STDIN +#define USE_SERIAL_4_STDIN 0 +#endif +#ifndef USE_SERIAL_5_STDIN +#define USE_SERIAL_5_STDIN 0 +#endif +#ifndef USE_SERIAL_6_STDIN +#define USE_SERIAL_6_STDIN 0 +#endif +#ifndef USE_SERIAL_7_STDIN +#define USE_SERIAL_7_STDIN 0 +#endif +#ifndef USE_SERIAL_8_STDIN +#define USE_SERIAL_8_STDIN 0 +#endif +#ifndef USE_SERIAL_9_STDIN +#define USE_SERIAL_9_STDIN 0 +#endif +#ifndef PACKET_HAS_SLAVE +#define PACKET_HAS_SLAVE 0 +#endif + +#define USE_SERIAL_STDIN (USE_SERIAL_0_STDIN + \ + USE_SERIAL_1_STDIN + \ + USE_SERIAL_2_STDIN + \ + USE_SERIAL_3_STDIN + \ + USE_SERIAL_4_STDIN + \ + USE_SERIAL_5_STDIN + \ + USE_SERIAL_6_STDIN + \ + USE_SERIAL_7_STDIN + \ + USE_SERIAL_8_STDIN + \ + USE_SERIAL_9_STDIN) + +#define AO_NUM_STDIOS (HAS_USB + PACKET_HAS_SLAVE + USE_SERIAL_STDIN) + +__xdata struct ao_stdio ao_stdios[AO_NUM_STDIOS]; + +#if AO_NUM_STDIOS > 1 +__pdata int8_t ao_cur_stdio; +__pdata int8_t ao_num_stdios; +#else +__pdata int8_t ao_cur_stdio; +#define ao_cur_stdio 0 +#define ao_num_stdios 0 +#endif + +void +putchar(char c) +{ +#if LOW_LEVEL_DEBUG + if (!ao_cur_task) { + extern void ao_debug_out(char c); + if (c == '\n') + ao_debug_out('\r'); + ao_debug_out(c); + return; + } +#endif + if (c == '\n') + (*ao_stdios[ao_cur_stdio].putchar)('\r'); + (*ao_stdios[ao_cur_stdio].putchar)(c); +} + +void +flush(void) +{ + if (ao_stdios[ao_cur_stdio].flush) + ao_stdios[ao_cur_stdio].flush(); +} + +__xdata uint8_t ao_stdin_ready; + +char +getchar(void) __reentrant +{ + int c; + int8_t stdio; + + ao_arch_block_interrupts(); + stdio = ao_cur_stdio; + for (;;) { + c = ao_stdios[stdio]._pollchar(); + if (c != AO_READ_AGAIN) + break; +#if AO_NUM_STDIOS > 1 + if (++stdio == ao_num_stdios) + stdio = 0; + if (stdio == ao_cur_stdio) +#endif + ao_sleep(&ao_stdin_ready); + } +#if AO_NUM_STDIOS > 1 + ao_cur_stdio = stdio; +#endif + ao_arch_release_interrupts(); + return c; +} + +uint8_t +ao_echo(void) +{ + return ao_stdios[ao_cur_stdio].echo; +} + +int8_t +ao_add_stdio(int (*_pollchar)(void), + void (*putchar)(char), + void (*flush)(void)) __reentrant +{ +#if AO_NUM_STDIOS > 1 + if (ao_num_stdios == AO_NUM_STDIOS) + ao_panic(AO_PANIC_STDIO); +#endif + ao_stdios[ao_num_stdios]._pollchar = _pollchar; + ao_stdios[ao_num_stdios].putchar = putchar; + ao_stdios[ao_num_stdios].flush = flush; + ao_stdios[ao_num_stdios].echo = 1; +#if AO_NUM_STDIOS > 1 + return ao_num_stdios++; +#else + return 0; +#endif +} diff --git a/src/kernel/ao_storage.c b/src/kernel/ao_storage.c new file mode 100644 index 00000000..6eddae7f --- /dev/null +++ b/src/kernel/ao_storage.c @@ -0,0 +1,186 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +uint8_t +ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant +{ + uint16_t this_len; + uint16_t this_off; + + ao_storage_setup(); + if (pos >= ao_storage_total || pos + len > ao_storage_total) + return 0; + while (len) { + + /* Compute portion of transfer within + * a single block + */ + this_off = (uint16_t) pos & (ao_storage_unit - 1); + this_len = ao_storage_unit - this_off; + if (this_len > len) + this_len = len; + + if (!ao_storage_device_read(pos, buf, this_len)) + return 0; + + /* See how much is left */ + buf += this_len; + len -= this_len; + pos += this_len; + } + return 1; +} + +uint8_t +ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant +{ + uint16_t this_len; + uint16_t this_off; + + ao_storage_setup(); + if (pos >= ao_storage_total || pos + len > ao_storage_total) + return 0; + while (len) { + + /* Compute portion of transfer within + * a single block + */ + this_off = (uint16_t) pos & (ao_storage_unit - 1); + this_len = ao_storage_unit - this_off; + if (this_len > len) + this_len = len; + + if (!ao_storage_device_write(pos, buf, this_len)) + return 0; + + /* See how much is left */ + buf += this_len; + len -= this_len; + pos += this_len; + } + return 1; +} + +static __xdata uint8_t storage_data[8]; + +static void +ao_storage_dump(void) __reentrant +{ + uint8_t i, j; + + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + for (i = 0; ; i += 8) { + if (ao_storage_read(((uint32_t) (ao_cmd_lex_i) << 8) + i, + storage_data, + 8)) { + ao_cmd_put16((uint16_t) i); + for (j = 0; j < 8; j++) { + putchar(' '); + ao_cmd_put8(storage_data[j]); + } + putchar ('\n'); + } + if (i == 248) + break; + } +} + +#if HAS_STORAGE_DEBUG + +/* not enough space for this today + */ +static void +ao_storage_store(void) __reentrant +{ + uint16_t block; + uint8_t i; + uint16_t len; + static __xdata uint8_t b; + uint32_t addr; + + ao_cmd_hex(); + block = ao_cmd_lex_i; + ao_cmd_hex(); + i = ao_cmd_lex_i; + addr = ((uint32_t) block << 8) | i; + ao_cmd_hex(); + len = ao_cmd_lex_i; + if (ao_cmd_status != ao_cmd_success) + return; + while (len--) { + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + b = ao_cmd_lex_i; + ao_storage_write(addr, &b, 1); + addr++; + } +} +#endif + +void +ao_storage_zap(void) __reentrant +{ + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_storage_erase((uint32_t) ao_cmd_lex_i << 8); +} + +void +ao_storage_zapall(void) __reentrant +{ + uint32_t pos; + + ao_cmd_white(); + if (!ao_match_word("DoIt")) + return; + for (pos = 0; pos < ao_storage_log_max; pos += ao_storage_block) + ao_storage_erase(pos); +} + +void +ao_storage_info(void) __reentrant +{ + ao_storage_setup(); + printf("Storage size: %ld\n", (long) ao_storage_total); + printf("Storage erase unit: %ld\n", (long) ao_storage_block); + ao_storage_device_info(); +} + +__code struct ao_cmds ao_storage_cmds[] = { + { ao_storage_info, "f\0Show storage" }, + { ao_storage_dump, "e \0Dump flash" }, +#if HAS_STORAGE_DEBUG + { ao_storage_store, "w ...\0Write data to flash" }, +#endif + { ao_storage_zap, "z \0Erase " }, + { ao_storage_zapall,"Z \0Erase all. is doit with D&I" }, + { 0, NULL }, +}; + +void +ao_storage_init(void) +{ + ao_storage_device_init(); + ao_cmd_register(&ao_storage_cmds[0]); +} diff --git a/src/kernel/ao_storage.h b/src/kernel/ao_storage.h new file mode 100644 index 00000000..6cc6fcb7 --- /dev/null +++ b/src/kernel/ao_storage.h @@ -0,0 +1,98 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_STORAGE_H_ +#define _AO_STORAGE_H_ + +/* + * Storage interface, provided by one of the eeprom or flash + * drivers + */ + +#ifndef ao_storage_pos_t +#define ao_storage_pos_t uint32_t +#endif + +typedef ao_storage_pos_t ao_pos_t; + +/* Total bytes of available storage */ +extern __pdata ao_pos_t ao_storage_total; + +/* Block size - device is erased in these units. At least 256 bytes */ +extern __pdata ao_pos_t ao_storage_block; + +#ifndef USE_STORAGE_CONFIG +#define USE_STORAGE_CONFIG 1 +#endif + +#if USE_STORAGE_CONFIG +/* Byte offset of config block. Will be ao_storage_block bytes long */ +extern __pdata ao_pos_t ao_storage_config; + +#define ao_storage_log_max ao_storage_config +#else +#define ao_storage_log_max ao_storage_total +#endif + +/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */ +extern __pdata uint16_t ao_storage_unit; + +/* Initialize above values. Can only be called once the OS is running */ +void +ao_storage_setup(void) __reentrant; + +/* Write data. Returns 0 on failure, 1 on success */ +uint8_t +ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; + +/* Read data. Returns 0 on failure, 1 on success */ +uint8_t +ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; + +/* Erase a block of storage. This always clears ao_storage_block bytes */ +uint8_t +ao_storage_erase(ao_pos_t pos) __reentrant; + +/* Flush any pending writes to stable storage */ +void +ao_storage_flush(void) __reentrant; + +/* Initialize the storage code */ +void +ao_storage_init(void); + +/* + * Low-level functions wrapped by ao_storage.c + */ + +/* Read data within a storage unit */ +uint8_t +ao_storage_device_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; + +/* Write data within a storage unit */ +uint8_t +ao_storage_device_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; + +/* Initialize low-level device bits */ +void +ao_storage_device_init(void); + +/* Print out information about flash chips */ +void +ao_storage_device_info(void) __reentrant; + +#endif /* _AO_STORAGE_H_ */ diff --git a/src/kernel/ao_task.c b/src/kernel/ao_task.c new file mode 100644 index 00000000..bafb4943 --- /dev/null +++ b/src/kernel/ao_task.c @@ -0,0 +1,548 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#if HAS_SAMPLE_PROFILE +#include +#endif +#if HAS_STACK_GUARD +#include +#endif + +#define DEBUG 0 + +#define AO_NO_TASK_INDEX 0xff + +__xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; +__data uint8_t ao_num_tasks; +__xdata struct ao_task *__data ao_cur_task; + +#if !HAS_TASK_QUEUE +static __data uint8_t ao_cur_task_index; +#endif + +#ifdef ao_arch_task_globals +ao_arch_task_globals +#endif + +#define AO_CHECK_STACK 0 + +#if AO_CHECK_STACK +static uint8_t in_yield; + +static inline void ao_check_stack(void) { + uint8_t q; + if (!in_yield && ao_cur_task && &q < &ao_cur_task->stack[0]) + ao_panic(AO_PANIC_STACK); +} +#else +#define ao_check_stack() +#endif + +#if HAS_TASK_QUEUE + +#define SLEEP_HASH_SIZE 17 + +static struct ao_list run_queue; +static struct ao_list alarm_queue; +static struct ao_list sleep_queue[SLEEP_HASH_SIZE]; + +static void +ao_task_to_run_queue(struct ao_task *task) +{ + ao_list_del(&task->queue); + ao_list_append(&task->queue, &run_queue); +} + +static struct ao_list * +ao_task_sleep_queue(void *wchan) +{ + return &sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE]; +} + +static void +ao_task_to_sleep_queue(struct ao_task *task, void *wchan) +{ + ao_list_del(&task->queue); + ao_list_append(&task->queue, ao_task_sleep_queue(wchan)); +} + +#if DEBUG +static void +ao_task_validate_alarm_queue(void) +{ + struct ao_task *alarm, *prev = NULL; + int i; + + if (ao_list_is_empty(&alarm_queue)) + return; + ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { + if (prev) { + if ((int16_t) (alarm->alarm - prev->alarm) < 0) { + ao_panic(1); + } + } + prev = alarm; + } + for (i = 0; i < ao_num_tasks; i++) { + alarm = ao_tasks[i]; + if (alarm->alarm) { + if (ao_list_is_empty(&alarm->alarm_queue)) + ao_panic(2); + } else { + if (!ao_list_is_empty(&alarm->alarm_queue)) + ao_panic(3); + } + } + if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm) + ao_panic(4); +} +#else +#define ao_task_validate_alarm_queue() +#endif + +uint16_t ao_task_alarm_tick; + +static void +ao_task_to_alarm_queue(struct ao_task *task) +{ + struct ao_task *alarm; + ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { + if ((int16_t) (alarm->alarm - task->alarm) >= 0) { + ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev); + ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; + ao_task_validate_alarm_queue(); + return; + } + } + ao_list_append(&task->alarm_queue, &alarm_queue); + ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; + ao_task_validate_alarm_queue(); +} + +static void +ao_task_from_alarm_queue(struct ao_task *task) +{ + ao_list_del(&task->alarm_queue); + if (ao_list_is_empty(&alarm_queue)) + ao_task_alarm_tick = 0; + else + ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; + ao_task_validate_alarm_queue(); +} + +static void +ao_task_init_queue(struct ao_task *task) +{ + ao_list_init(&task->queue); + ao_list_init(&task->alarm_queue); +} + +static void +ao_task_exit_queue(struct ao_task *task) +{ + ao_list_del(&task->queue); + ao_list_del(&task->alarm_queue); +} + +void +ao_task_check_alarm(uint16_t tick) +{ + struct ao_task *alarm, *next; + + ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) { + if ((int16_t) (tick - alarm->alarm) < 0) + break; + alarm->alarm = 0; + ao_task_from_alarm_queue(alarm); + ao_task_to_run_queue(alarm); + } +} + +void +ao_task_init(void) +{ + uint8_t i; + ao_list_init(&run_queue); + ao_list_init(&alarm_queue); + ao_task_alarm_tick = 0; + for (i = 0; i < SLEEP_HASH_SIZE; i++) + ao_list_init(&sleep_queue[i]); +} + +#if DEBUG +static uint8_t +ao_task_validate_queue(struct ao_task *task) +{ + uint32_t flags; + struct ao_task *m; + uint8_t ret = 0; + struct ao_list *queue; + + flags = ao_arch_irqsave(); + if (task->wchan) { + queue = ao_task_sleep_queue(task->wchan); + ret |= 2; + } else { + queue = &run_queue; + ret |= 4; + } + ao_list_for_each_entry(m, queue, struct ao_task, queue) { + if (m == task) { + ret |= 1; + break; + } + } + ao_arch_irqrestore(flags); + return ret; +} + +static uint8_t +ao_task_validate_alarm(struct ao_task *task) +{ + uint32_t flags; + struct ao_task *m; + uint8_t ret = 0; + + flags = ao_arch_irqsave(); + if (task->alarm == 0) + return 0xff; + ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) { + if (m == task) + ret |= 1; + else { + if (!(ret&1)) { + if ((int16_t) (m->alarm - task->alarm) > 0) + ret |= 2; + } else { + if ((int16_t) (task->alarm - m->alarm) > 0) + ret |= 4; + } + } + } + ao_arch_irqrestore(flags); + return ret; +} + + +static void +ao_task_validate(void) +{ + uint8_t i; + struct ao_task *task; + uint8_t ret; + + for (i = 0; i < ao_num_tasks; i++) { + task = ao_tasks[i]; + ret = ao_task_validate_queue(task); + if (!(ret & 1)) { + if (ret & 2) + printf ("sleeping task not on sleep queue %s %08x\n", + task->name, task->wchan); + else + printf ("running task not on run queue %s\n", + task->name); + } + ret = ao_task_validate_alarm(task); + if (ret != 0xff) { + if (!(ret & 1)) + printf ("alarm task not on alarm queue %s %d\n", + task->name, task->alarm); + if (ret & 2) + printf ("alarm queue has sooner entries after %s %d\n", + task->name, task->alarm); + if (ret & 4) + printf ("alarm queue has later entries before %s %d\n", + task->name, task->alarm); + } + } +} +#endif /* DEBUG */ + +#endif /* HAS_TASK_QUEUE */ + +void +ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant +{ + uint8_t task_id; + uint8_t t; + if (ao_num_tasks == AO_NUM_TASKS) + ao_panic(AO_PANIC_NO_TASK); + for (task_id = 1; task_id != 0; task_id++) { + for (t = 0; t < ao_num_tasks; t++) + if (ao_tasks[t]->task_id == task_id) + break; + if (t == ao_num_tasks) + break; + } + task->task_id = task_id; + task->name = name; + task->wchan = NULL; + /* + * Construct a stack frame so that it will 'return' + * to the start of the task + */ + ao_arch_init_stack(task, start); + ao_arch_critical( +#if HAS_TASK_QUEUE + ao_task_init_queue(task); + ao_task_to_run_queue(task); +#endif + ao_tasks[ao_num_tasks] = task; + ao_num_tasks++; + ); +} + +__data uint8_t ao_task_minimize_latency; + +/* Task switching function. This must not use any stack variables */ +void +ao_yield(void) ao_arch_naked_define +{ + ao_arch_save_regs(); + +#if HAS_TASK_QUEUE + if (ao_cur_task == NULL) + ao_cur_task = ao_tasks[ao_num_tasks-1]; +#else + if (ao_cur_task_index == AO_NO_TASK_INDEX) + ao_cur_task_index = ao_num_tasks-1; +#endif + else + { +#if HAS_SAMPLE_PROFILE + uint16_t tick = ao_sample_profile_timer_value(); + uint16_t run = tick - ao_cur_task->start; + if (run > ao_cur_task->max_run) + ao_cur_task->max_run = run; + ++ao_cur_task->yields; +#endif + ao_arch_save_stack(); + } + + ao_arch_isr_stack(); +#if !HAS_TASK_QUEUE + if (ao_task_minimize_latency) + ao_arch_release_interrupts(); + else +#endif + ao_arch_block_interrupts(); + +#if AO_CHECK_STACK + in_yield = 1; +#endif + /* Find a task to run. If there isn't any runnable task, + * this loop will run forever, which is just fine + */ +#if HAS_TASK_QUEUE + /* If the current task is running, move it to the + * end of the queue to allow other tasks a chance + */ + if (ao_cur_task->wchan == NULL) + ao_task_to_run_queue(ao_cur_task); + ao_cur_task = NULL; + for (;;) { + ao_arch_memory_barrier(); + if (!ao_list_is_empty(&run_queue)) + break; + /* Wait for interrupts when there's nothing ready */ + ao_arch_wait_interrupt(); + } + ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue); +#else + { + __pdata uint8_t ao_last_task_index = ao_cur_task_index; + for (;;) { + ++ao_cur_task_index; + if (ao_cur_task_index == ao_num_tasks) + ao_cur_task_index = 0; + + ao_cur_task = ao_tasks[ao_cur_task_index]; + + /* Check for ready task */ + if (ao_cur_task->wchan == NULL) + break; + + /* Check if the alarm is set for a time which has passed */ + if (ao_cur_task->alarm && + (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) + break; + + /* Wait for interrupts when there's nothing ready */ + if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency) + ao_arch_wait_interrupt(); + } + } +#endif +#if HAS_SAMPLE_PROFILE + ao_cur_task->start = ao_sample_profile_timer_value(); +#endif +#if HAS_STACK_GUARD + ao_mpu_stack_guard(ao_cur_task->stack); +#endif +#if AO_CHECK_STACK + in_yield = 0; +#endif + ao_arch_restore_stack(); +} + +uint8_t +ao_sleep(__xdata void *wchan) +{ +#if HAS_TASK_QUEUE + uint32_t flags; + flags = ao_arch_irqsave(); +#endif + ao_cur_task->wchan = wchan; +#if HAS_TASK_QUEUE + ao_task_to_sleep_queue(ao_cur_task, wchan); + ao_arch_irqrestore(flags); +#endif + ao_yield(); + if (ao_cur_task->wchan) { + ao_cur_task->wchan = NULL; + ao_cur_task->alarm = 0; + return 1; + } + return 0; +} + +void +ao_wakeup(__xdata void *wchan) __reentrant +{ +#if HAS_TASK_QUEUE + struct ao_task *sleep, *next; + struct ao_list *sleep_queue; + uint32_t flags; + + if (ao_num_tasks == 0) + return; + sleep_queue = ao_task_sleep_queue(wchan); + flags = ao_arch_irqsave(); + ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) { + if (sleep->wchan == wchan) { + sleep->wchan = NULL; + ao_task_to_run_queue(sleep); + } + } + ao_arch_irqrestore(flags); +#else + uint8_t i; + for (i = 0; i < ao_num_tasks; i++) + if (ao_tasks[i]->wchan == wchan) + ao_tasks[i]->wchan = NULL; +#endif + ao_check_stack(); +} + +void +ao_alarm(uint16_t delay) +{ +#if HAS_TASK_QUEUE + uint32_t flags; + /* Make sure we sleep *at least* delay ticks, which means adding + * one to account for the fact that we may be close to the next tick + */ + flags = ao_arch_irqsave(); +#endif + if (!(ao_cur_task->alarm = ao_time() + delay + 1)) + ao_cur_task->alarm = 1; +#if HAS_TASK_QUEUE + ao_task_to_alarm_queue(ao_cur_task); + ao_arch_irqrestore(flags); +#endif +} + +void +ao_clear_alarm(void) +{ +#if HAS_TASK_QUEUE + uint32_t flags; + + flags = ao_arch_irqsave(); +#endif + ao_cur_task->alarm = 0; +#if HAS_TASK_QUEUE + ao_task_from_alarm_queue(ao_cur_task); + ao_arch_irqrestore(flags); +#endif +} + +static __xdata uint8_t ao_forever; + +void +ao_delay(uint16_t ticks) +{ + ao_alarm(ticks); + ao_sleep(&ao_forever); + ao_clear_alarm(); +} + +void +ao_exit(void) +{ + uint8_t i; + ao_arch_block_interrupts(); + ao_num_tasks--; +#if HAS_TASK_QUEUE + for (i = 0; i < ao_num_tasks; i++) + if (ao_tasks[i] == ao_cur_task) + break; + ao_task_exit_queue(ao_cur_task); +#else + i = ao_cur_task_index; + ao_cur_task_index = AO_NO_TASK_INDEX; +#endif + for (; i < ao_num_tasks; i++) + ao_tasks[i] = ao_tasks[i+1]; + ao_cur_task = NULL; + ao_yield(); + /* we'll never get back here */ +} + +#if HAS_TASK_INFO +void +ao_task_info(void) +{ + uint8_t i; + __xdata struct ao_task *task; + + for (i = 0; i < ao_num_tasks; i++) { + task = ao_tasks[i]; + printf("%12s: wchan %04x\n", + task->name, + (int) task->wchan); + } +#if HAS_TASK_QUEUE && DEBUG + ao_task_validate(); +#endif +} +#endif + +void +ao_start_scheduler(void) +{ +#if !HAS_TASK_QUEUE + ao_cur_task_index = AO_NO_TASK_INDEX; +#endif + ao_cur_task = NULL; +#if HAS_ARCH_START_SCHEDULER + ao_arch_start_scheduler(); +#endif + ao_yield(); +} diff --git a/src/kernel/ao_task.h b/src/kernel/ao_task.h new file mode 100644 index 00000000..9c56b480 --- /dev/null +++ b/src/kernel/ao_task.h @@ -0,0 +1,117 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_TASK_H_ +#define _AO_TASK_H_ +#if HAS_TASK_QUEUE +#include +#endif + +#ifndef HAS_TASK_INFO +#define HAS_TASK_INFO 1 +#endif + +/* An AltOS task */ +struct ao_task { + __xdata void *wchan; /* current wait channel (NULL if running) */ + uint16_t alarm; /* abort ao_sleep time */ + ao_arch_task_members /* any architecture-specific fields */ + uint8_t task_id; /* unique id */ + __code char *name; /* task name */ +#if HAS_TASK_QUEUE + struct ao_list queue; + struct ao_list alarm_queue; +#endif + uint8_t stack[AO_STACK_SIZE]; /* saved stack */ +#if HAS_SAMPLE_PROFILE + uint32_t ticks; + uint32_t yields; + uint16_t start; + uint16_t max_run; +#endif +}; + +#ifndef AO_NUM_TASKS +#define AO_NUM_TASKS 16 /* maximum number of tasks */ +#endif + +#define AO_NO_TASK 0 /* no task id */ + +extern __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; +extern __data uint8_t ao_num_tasks; +extern __xdata struct ao_task *__data ao_cur_task; +extern __data uint8_t ao_task_minimize_latency; /* Reduce IRQ latency */ + +/* + ao_task.c + */ + +/* Suspend the current task until wchan is awoken. + * returns: + * 0 on normal wake + * 1 on alarm + */ +uint8_t +ao_sleep(__xdata void *wchan); + +/* Wake all tasks sleeping on wchan */ +void +ao_wakeup(__xdata void *wchan) __reentrant; + +/* set an alarm to go off in 'delay' ticks */ +void +ao_alarm(uint16_t delay); + +/* Clear any pending alarm */ +void +ao_clear_alarm(void); + +/* Yield the processor to another task */ +void +ao_yield(void) ao_arch_naked_declare; + +/* Add a task to the run queue */ +void +ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant; + +#if HAS_TASK_QUEUE +/* Called on timer interrupt to check alarms */ +extern uint16_t ao_task_alarm_tick; +void +ao_task_check_alarm(uint16_t tick); +#endif + +/* Terminate the current task */ +void +ao_exit(void); + +/* Dump task info to console */ +void +ao_task_info(void); + +/* Start the scheduler. This will not return */ +void +ao_start_scheduler(void); + +#if HAS_TASK_QUEUE +void +ao_task_init(void); +#else +#define ao_task_init() +#endif + +#endif diff --git a/src/kernel/ao_telem.h b/src/kernel/ao_telem.h new file mode 100644 index 00000000..1a8da291 --- /dev/null +++ b/src/kernel/ao_telem.h @@ -0,0 +1,172 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_TELEM_H_ +#define _AO_TELEM_H_ + +#define AO_TELEMETRY_VERSION 4 + +/* + * Telemetry version 4 and higher format: + * + * General header fields + * + * Name Value + * + * VERSION Telemetry version number (4 or more). Must be first. + * c Callsign (string, no spaces allowed) + * n Flight unit serial number (integer) + * f Flight number (integer) + * r Packet RSSI value (integer) + * s Flight computer state (string, no spaces allowed) + * t Flight computer clock (integer in centiseconds) + */ + +#define AO_TELEM_VERSION "VERSION" +#define AO_TELEM_CALL "c" +#define AO_TELEM_SERIAL "n" +#define AO_TELEM_FLIGHT "f" +#define AO_TELEM_RSSI "r" +#define AO_TELEM_STATE "s" +#define AO_TELEM_TICK "t" + +/* + * Raw sensor values + * + * Name Value + * r_a Accelerometer reading (integer) + * r_b Barometer reading (integer) + * r_t Thermometer reading (integer) + * r_v Battery reading (integer) + * r_d Drogue continuity (integer) + * r_m Main continuity (integer) + */ + +#define AO_TELEM_RAW_ACCEL "r_a" +#define AO_TELEM_RAW_BARO "r_b" +#define AO_TELEM_RAW_THERMO "r_t" +#define AO_TELEM_RAW_BATT "r_v" +#define AO_TELEM_RAW_DROGUE "r_d" +#define AO_TELEM_RAW_MAIN "r_m" + +/* + * Sensor calibration values + * + * Name Value + * c_a Ground accelerometer reading (integer) + * c_b Ground barometer reading (integer) + * c_p Accelerometer reading for +1g + * c_m Accelerometer reading for -1g + */ + +#define AO_TELEM_CAL_ACCEL_GROUND "c_a" +#define AO_TELEM_CAL_BARO_GROUND "c_b" +#define AO_TELEM_CAL_ACCEL_PLUS "c_p" +#define AO_TELEM_CAL_ACCEL_MINUS "c_m" + +/* + * Kalman state values + * + * Name Value + * k_h Height above pad (integer, meters) + * k_s Vertical speeed (integer, m/s * 16) + * k_a Vertical acceleration (integer, m/s² * 16) + */ + +#define AO_TELEM_KALMAN_HEIGHT "k_h" +#define AO_TELEM_KALMAN_SPEED "k_s" +#define AO_TELEM_KALMAN_ACCEL "k_a" + +/* + * Ad-hoc flight values + * + * Name Value + * a_a Acceleration (integer, sensor units) + * a_s Speed (integer, integrated acceleration value) + * a_b Barometer reading (integer, sensor units) + */ + +#define AO_TELEM_ADHOC_ACCEL "a_a" +#define AO_TELEM_ADHOC_SPEED "a_s" +#define AO_TELEM_ADHOC_BARO "a_b" + +/* + * GPS values + * + * Name Value + * g GPS state (string): + * l locked + * u unlocked + * e error (missing or broken) + * g_n Number of sats used in solution + * g_ns Latitude (degrees * 10e7) + * g_ew Longitude (degrees * 10e7) + * g_a Altitude (integer meters) + * g_Y GPS year (integer) + * g_M GPS month (integer - 1-12) + * g_D GPS day (integer - 1-31) + * g_h GPS hour (integer - 0-23) + * g_m GPS minute (integer - 0-59) + * g_s GPS second (integer - 0-59) + * g_v GPS vertical speed (integer, cm/sec) + * g_g GPS horizontal speed (integer, cm/sec) + * g_c GPS course (integer, 0-359) + * g_hd GPS hdop (integer * 10) + * g_vd GPS vdop (integer * 10) + * g_he GPS h error (integer) + * g_ve GPS v error (integer) + */ + +#define AO_TELEM_GPS_STATE "g" +#define AO_TELEM_GPS_STATE_LOCKED 'l' +#define AO_TELEM_GPS_STATE_UNLOCKED 'u' +#define AO_TELEM_GPS_STATE_ERROR 'e' +#define AO_TELEM_GPS_NUM_SAT "g_n" +#define AO_TELEM_GPS_LATITUDE "g_ns" +#define AO_TELEM_GPS_LONGITUDE "g_ew" +#define AO_TELEM_GPS_ALTITUDE "g_a" +#define AO_TELEM_GPS_YEAR "g_Y" +#define AO_TELEM_GPS_MONTH "g_M" +#define AO_TELEM_GPS_DAY "g_D" +#define AO_TELEM_GPS_HOUR "g_h" +#define AO_TELEM_GPS_MINUTE "g_m" +#define AO_TELEM_GPS_SECOND "g_s" +#define AO_TELEM_GPS_VERTICAL_SPEED "g_v" +#define AO_TELEM_GPS_HORIZONTAL_SPEED "g_g" +#define AO_TELEM_GPS_COURSE "g_c" +#define AO_TELEM_GPS_HDOP "g_hd" +#define AO_TELEM_GPS_VDOP "g_vd" +#define AO_TELEM_GPS_HERROR "g_he" +#define AO_TELEM_GPS_VERROR "g_ve" + +/* + * GPS satellite values + * + * Name Value + * s_n Number of satellites reported (integer) + * s_v0 Space vehicle ID (integer) for report 0 + * s_c0 C/N0 number (integer) for report 0 + * s_v1 Space vehicle ID (integer) for report 1 + * s_c1 C/N0 number (integer) for report 1 + * ... + */ + +#define AO_TELEM_SAT_NUM "s_n" +#define AO_TELEM_SAT_SVID "s_v" +#define AO_TELEM_SAT_C_N_0 "s_c" + +#endif /* _AO_TELEM_H_ */ diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c new file mode 100644 index 00000000..a1c19185 --- /dev/null +++ b/src/kernel/ao_telemetry.c @@ -0,0 +1,555 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_log.h" +#include "ao_product.h" + +#ifndef HAS_RDF +#define HAS_RDF 1 +#endif + +static __pdata uint16_t ao_telemetry_interval; + +#if HAS_RDF +static __pdata uint8_t ao_rdf = 0; +static __pdata uint16_t ao_rdf_time; +#endif + +#if HAS_APRS +static __pdata uint16_t ao_aprs_time; + +#include +#endif + +#if defined(TELEMEGA) +#define AO_SEND_MEGA 1 +#endif + +#if defined (TELEMETRUM_V_2_0) +#define AO_SEND_METRUM 1 +#endif + +#if defined(TELEMETRUM_V_0_1) || defined(TELEMETRUM_V_0_2) || defined(TELEMETRUM_V_1_0) || defined(TELEMETRUM_V_1_1) || defined(TELEBALLOON_V_1_1) || defined(TELEMETRUM_V_1_2) +#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMETRUM +#endif + +#if defined(TELEMINI_V_1_0) +#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMINI +#endif + +#if defined(TELENANO_V_0_1) +#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELENANO +#endif + +static __xdata union ao_telemetry_all telemetry; + +#if defined AO_TELEMETRY_SENSOR +/* Send sensor packet */ +static void +ao_send_sensor(void) +{ + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_SENSOR; + + telemetry.sensor.state = ao_flight_state; +#if HAS_ACCEL + telemetry.sensor.accel = packet->adc.accel; +#else + telemetry.sensor.accel = 0; +#endif + telemetry.sensor.pres = ao_data_pres(packet); + telemetry.sensor.temp = packet->adc.temp; + telemetry.sensor.v_batt = packet->adc.v_batt; +#if HAS_IGNITE + telemetry.sensor.sense_d = packet->adc.sense_d; + telemetry.sensor.sense_m = packet->adc.sense_m; +#else + telemetry.sensor.sense_d = 0; + telemetry.sensor.sense_m = 0; +#endif + + telemetry.sensor.acceleration = ao_accel; + telemetry.sensor.speed = ao_speed; + telemetry.sensor.height = ao_height; + + telemetry.sensor.ground_pres = ao_ground_pres; +#if HAS_ACCEL + telemetry.sensor.ground_accel = ao_ground_accel; + telemetry.sensor.accel_plus_g = ao_config.accel_plus_g; + telemetry.sensor.accel_minus_g = ao_config.accel_minus_g; +#else + telemetry.sensor.ground_accel = 0; + telemetry.sensor.accel_plus_g = 0; + telemetry.sensor.accel_minus_g = 0; +#endif + + ao_radio_send(&telemetry, sizeof (telemetry)); +} +#endif + + +#ifdef AO_SEND_MEGA +/* Send mega sensor packet */ +static void +ao_send_mega_sensor(void) +{ + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; + + telemetry.mega_sensor.orient = ao_sample_orient; + telemetry.mega_sensor.accel = ao_data_accel(packet); + telemetry.mega_sensor.pres = ao_data_pres(packet); + telemetry.mega_sensor.temp = ao_data_temp(packet); + +#if HAS_MPU6000 + telemetry.mega_sensor.accel_x = packet->mpu6000.accel_x; + telemetry.mega_sensor.accel_y = packet->mpu6000.accel_y; + telemetry.mega_sensor.accel_z = packet->mpu6000.accel_z; + + telemetry.mega_sensor.gyro_x = packet->mpu6000.gyro_x; + telemetry.mega_sensor.gyro_y = packet->mpu6000.gyro_y; + telemetry.mega_sensor.gyro_z = packet->mpu6000.gyro_z; +#endif + +#if HAS_HMC5883 + telemetry.mega_sensor.mag_x = packet->hmc5883.x; + telemetry.mega_sensor.mag_y = packet->hmc5883.y; + telemetry.mega_sensor.mag_z = packet->hmc5883.z; +#endif + + ao_radio_send(&telemetry, sizeof (telemetry)); +} + +static __pdata int8_t ao_telemetry_mega_data_max; +static __pdata int8_t ao_telemetry_mega_data_cur; + +/* Send mega data packet */ +static void +ao_send_mega_data(void) +{ + if (--ao_telemetry_mega_data_cur <= 0) { + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + uint8_t i; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_MEGA_DATA; + + telemetry.mega_data.state = ao_flight_state; + telemetry.mega_data.v_batt = packet->adc.v_batt; + telemetry.mega_data.v_pyro = packet->adc.v_pbatt; + + /* ADC range is 0-4095, so shift by four to save the high 8 bits */ + for (i = 0; i < AO_ADC_NUM_SENSE; i++) + telemetry.mega_data.sense[i] = packet->adc.sense[i] >> 4; + + telemetry.mega_data.ground_pres = ao_ground_pres; + telemetry.mega_data.ground_accel = ao_ground_accel; + telemetry.mega_data.accel_plus_g = ao_config.accel_plus_g; + telemetry.mega_data.accel_minus_g = ao_config.accel_minus_g; + + telemetry.mega_data.acceleration = ao_accel; + telemetry.mega_data.speed = ao_speed; + telemetry.mega_data.height = ao_height; + + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_mega_data_cur = ao_telemetry_mega_data_max; + } +} +#endif /* AO_SEND_MEGA */ + +#ifdef AO_SEND_METRUM +/* Send telemetrum sensor packet */ +static void +ao_send_metrum_sensor(void) +{ + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_METRUM_SENSOR; + + telemetry.metrum_sensor.state = ao_flight_state; +#if HAS_ACCEL + telemetry.metrum_sensor.accel = ao_data_accel(packet); +#endif + telemetry.metrum_sensor.pres = ao_data_pres(packet); + telemetry.metrum_sensor.temp = ao_data_temp(packet); + + telemetry.metrum_sensor.acceleration = ao_accel; + telemetry.metrum_sensor.speed = ao_speed; + telemetry.metrum_sensor.height = ao_height; + + telemetry.metrum_sensor.v_batt = packet->adc.v_batt; + telemetry.metrum_sensor.sense_a = packet->adc.sense_a; + telemetry.metrum_sensor.sense_m = packet->adc.sense_m; + + ao_radio_send(&telemetry, sizeof (telemetry)); +} + +static __pdata int8_t ao_telemetry_metrum_data_max; +static __pdata int8_t ao_telemetry_metrum_data_cur; + +/* Send telemetrum data packet */ +static void +ao_send_metrum_data(void) +{ + if (--ao_telemetry_metrum_data_cur <= 0) { + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_METRUM_DATA; + + telemetry.metrum_data.ground_pres = ao_ground_pres; +#if HAS_ACCEL + telemetry.metrum_data.ground_accel = ao_ground_accel; + telemetry.metrum_data.accel_plus_g = ao_config.accel_plus_g; + telemetry.metrum_data.accel_minus_g = ao_config.accel_minus_g; +#else + telemetry.metrum_data.ground_accel = 1; + telemetry.metrum_data.accel_plus_g = 0; + telemetry.metrum_data.accel_minus_g = 2; +#endif + + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_metrum_data_cur = ao_telemetry_metrum_data_max; + } +} +#endif /* AO_SEND_METRUM */ + +#ifdef AO_SEND_MINI + +static void +ao_send_mini(void) +{ + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_MINI; + + telemetry.mini.state = ao_flight_state; + + telemetry.mini.v_batt = packet->adc.v_batt; + telemetry.mini.sense_a = packet->adc.sense_a; + telemetry.mini.sense_m = packet->adc.sense_m; + + telemetry.mini.pres = ao_data_pres(packet); + telemetry.mini.temp = ao_data_temp(packet); + + telemetry.mini.acceleration = ao_accel; + telemetry.mini.speed = ao_speed; + telemetry.mini.height = ao_height; + + telemetry.mini.ground_pres = ao_ground_pres; + + ao_radio_send(&telemetry, sizeof (telemetry)); +} + +#endif /* AO_SEND_MINI */ + +#ifdef AO_SEND_ALL_BARO +static uint8_t ao_baro_sample; + +static void +ao_send_baro(void) +{ + uint8_t sample = ao_sample_data; + uint8_t samples = (sample - ao_baro_sample) & (AO_DATA_RING - 1); + + if (samples > 12) { + ao_baro_sample = (ao_baro_sample + (samples - 12)) & (AO_DATA_RING - 1); + samples = 12; + } + telemetry.generic.tick = ao_data_ring[sample].tick; + telemetry.generic.type = AO_TELEMETRY_BARO; + telemetry.baro.samples = samples; + for (sample = 0; sample < samples; sample++) { + telemetry.baro.baro[sample] = ao_data_ring[ao_baro_sample].adc.pres; + ao_baro_sample = ao_data_ring_next(ao_baro_sample); + } + ao_radio_send(&telemetry, sizeof (telemetry)); +} +#endif + +static __pdata int8_t ao_telemetry_config_max; +static __pdata int8_t ao_telemetry_config_cur; + +static void +ao_send_configuration(void) +{ + if (--ao_telemetry_config_cur <= 0) + { + telemetry.generic.type = AO_TELEMETRY_CONFIGURATION; + telemetry.configuration.device = AO_idProduct_NUMBER; +#if HAS_LOG + telemetry.configuration.flight = ao_log_full() ? 0 : ao_flight_number; +#else + telemetry.configuration.flight = ao_flight_number; +#endif + telemetry.configuration.config_major = AO_CONFIG_MAJOR; + telemetry.configuration.config_minor = AO_CONFIG_MINOR; + telemetry.configuration.apogee_delay = ao_config.apogee_delay; + telemetry.configuration.main_deploy = ao_config.main_deploy; + telemetry.configuration.flight_log_max = ao_config.flight_log_max >> 10; + ao_xmemcpy (telemetry.configuration.callsign, + ao_config.callsign, + AO_MAX_CALLSIGN); + ao_xmemcpy (telemetry.configuration.version, + CODE_TO_XDATA(ao_version), + AO_MAX_VERSION); + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_config_cur = ao_telemetry_config_max; + } +} + +#if HAS_GPS + +static __pdata int8_t ao_telemetry_loc_cur; +static __pdata int8_t ao_telemetry_sat_cur; + +static void +ao_send_location(void) +{ + if (--ao_telemetry_loc_cur <= 0) + { + telemetry.generic.type = AO_TELEMETRY_LOCATION; + ao_mutex_get(&ao_gps_mutex); + ao_xmemcpy(&telemetry.location.flags, + &ao_gps_data.flags, + 26); + telemetry.location.tick = ao_gps_tick; + ao_mutex_put(&ao_gps_mutex); + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_loc_cur = ao_telemetry_config_max; + } +} + +static void +ao_send_satellite(void) +{ + if (--ao_telemetry_sat_cur <= 0) + { + telemetry.generic.type = AO_TELEMETRY_SATELLITE; + ao_mutex_get(&ao_gps_mutex); + telemetry.satellite.channels = ao_gps_tracking_data.channels; + ao_xmemcpy(&telemetry.satellite.sats, + &ao_gps_tracking_data.sats, + AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info)); + ao_mutex_put(&ao_gps_mutex); + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_sat_cur = ao_telemetry_config_max; + } +} +#endif + +#if HAS_COMPANION + +static __pdata int8_t ao_telemetry_companion_max; +static __pdata int8_t ao_telemetry_companion_cur; + +static void +ao_send_companion(void) +{ + if (--ao_telemetry_companion_cur <= 0) { + telemetry.generic.type = AO_TELEMETRY_COMPANION; + telemetry.companion.board_id = ao_companion_setup.board_id; + telemetry.companion.update_period = ao_companion_setup.update_period; + telemetry.companion.channels = ao_companion_setup.channels; + ao_mutex_get(&ao_companion_mutex); + ao_xmemcpy(&telemetry.companion.companion_data, + ao_companion_data, + ao_companion_setup.channels * 2); + ao_mutex_put(&ao_companion_mutex); + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_companion_cur = ao_telemetry_companion_max; + } +} +#endif + +void +ao_telemetry(void) +{ + uint16_t time; + int16_t delay; + + ao_config_get(); + if (!ao_config.radio_enable) + ao_exit(); + while (!ao_flight_number) + ao_sleep(&ao_flight_number); + + telemetry.generic.serial = ao_serial_number; + for (;;) { + while (ao_telemetry_interval == 0) + ao_sleep(&telemetry); + time = ao_time(); +#if HAS_RDF + ao_rdf_time = time; +#endif +#if HAS_APRS + ao_aprs_time = time; +#endif + while (ao_telemetry_interval) { +#if HAS_APRS + if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) +#endif + { +#ifdef AO_SEND_ALL_BARO + ao_send_baro(); +#endif + +#if HAS_FLIGHT +# ifdef AO_SEND_MEGA + ao_send_mega_sensor(); + ao_send_mega_data(); +# endif +# ifdef AO_SEND_METRUM + ao_send_metrum_sensor(); + ao_send_metrum_data(); +# endif +# ifdef AO_SEND_MINI + ao_send_mini(); +# endif +# ifdef AO_TELEMETRY_SENSOR + ao_send_sensor(); +# endif +#endif /* HAS_FLIGHT */ + +#if HAS_COMPANION + if (ao_companion_running) + ao_send_companion(); +#endif + ao_send_configuration(); +#if HAS_GPS + ao_send_location(); + ao_send_satellite(); +#endif + } +#ifndef AO_SEND_ALL_BARO +#if HAS_RDF + if (ao_rdf && +#if HAS_APRS + !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && +#endif /* HAS_APRS */ + (int16_t) (ao_time() - ao_rdf_time) >= 0) + { +#if HAS_IGNITE_REPORT + uint8_t c; +#endif /* HAS_IGNITE_REPORT */ + ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; +#if HAS_IGNITE_REPORT + if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) + ao_radio_continuity(c); + else +#endif /* HAS_IGNITE_REPORT*/ + ao_radio_rdf(); + } +#endif /* HAS_RDF */ +#if HAS_APRS + if (ao_config.aprs_interval != 0 && + (int16_t) (ao_time() - ao_aprs_time) >= 0) + { + ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); + ao_aprs_send(); + } +#endif /* HAS_APRS */ +#endif /* !AO_SEND_ALL_BARO */ + time += ao_telemetry_interval; + delay = time - ao_time(); + if (delay > 0) { + ao_alarm(delay); + ao_sleep(&telemetry); + ao_clear_alarm(); + } + else + time = ao_time(); + } + } +} + +void +ao_telemetry_set_interval(uint16_t interval) +{ + int8_t cur = 0; + ao_telemetry_interval = interval; + +#if AO_SEND_MEGA + if (interval > 1) + ao_telemetry_mega_data_max = 1; + else + ao_telemetry_mega_data_max = 2; + if (ao_telemetry_mega_data_max > cur) + cur++; + ao_telemetry_mega_data_cur = cur; +#endif +#if AO_SEND_METRUM + ao_telemetry_metrum_data_max = AO_SEC_TO_TICKS(1) / interval; + if (ao_telemetry_metrum_data_max > cur) + cur++; + ao_telemetry_metrum_data_cur = cur; +#endif + +#if HAS_COMPANION + if (!ao_companion_setup.update_period) + ao_companion_setup.update_period = AO_SEC_TO_TICKS(1); + ao_telemetry_companion_max = ao_companion_setup.update_period / interval; + if (ao_telemetry_companion_max > cur) + cur++; + ao_telemetry_companion_cur = cur; +#endif + + ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval; +#if HAS_COMPANION + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_config_cur = cur; +#endif + +#if HAS_GPS + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_loc_cur = cur; + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_sat_cur = cur; +#endif + ao_wakeup(&telemetry); +} + +#if HAS_RDF +void +ao_rdf_set(uint8_t rdf) +{ + ao_rdf = rdf; + if (rdf == 0) + ao_radio_rdf_abort(); + else { + ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; + } +} +#endif + +__xdata struct ao_task ao_telemetry_task; + +void +ao_telemetry_init() +{ + ao_add_task(&ao_telemetry_task, ao_telemetry, "telemetry"); +} diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h new file mode 100644 index 00000000..237a35ab --- /dev/null +++ b/src/kernel/ao_telemetry.h @@ -0,0 +1,321 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_TELEMETRY_H_ +#define _AO_TELEMETRY_H_ + +/* + * ao_telemetry.c + */ +#define AO_MAX_CALLSIGN 8 +#define AO_MAX_VERSION 8 +#if LEGACY_MONITOR +#define AO_MAX_TELEMETRY 128 +#else +#define AO_MAX_TELEMETRY 32 +#endif + +struct ao_telemetry_generic { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + uint8_t payload[27]; /* 5 */ + /* 32 */ +}; + +#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01 +#define AO_TELEMETRY_SENSOR_TELEMINI 0x02 +#define AO_TELEMETRY_SENSOR_TELENANO 0x03 + +struct ao_telemetry_sensor { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + int16_t accel; /* 6 accelerometer (TM only) */ + int16_t pres; /* 8 pressure sensor */ + int16_t temp; /* 10 temperature sensor */ + int16_t v_batt; /* 12 battery voltage */ + int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */ + int16_t sense_m; /* 16 main continuity sense (TM/Tm) */ + + int16_t acceleration; /* 18 m/s² * 16 */ + int16_t speed; /* 20 m/s * 16 */ + int16_t height; /* 22 m */ + + int16_t ground_pres; /* 24 average pres on pad */ + int16_t ground_accel; /* 26 average accel on pad */ + int16_t accel_plus_g; /* 28 accel calibration at +1g */ + int16_t accel_minus_g; /* 30 accel calibration at -1g */ + /* 32 */ +}; + +#define AO_TELEMETRY_CONFIGURATION 0x04 + +struct ao_telemetry_configuration { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t device; /* 5 device type */ + uint16_t flight; /* 6 flight number */ + uint8_t config_major; /* 8 Config major version */ + uint8_t config_minor; /* 9 Config minor version */ + uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */ + uint16_t main_deploy; /* 12 Main deploy alt in meters */ + uint16_t flight_log_max; /* 14 Maximum flight log size in kB */ + char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */ + char version[AO_MAX_VERSION]; /* 24 Software version */ + /* 32 */ +}; + +#define AO_TELEMETRY_LOCATION 0x05 + +#define AO_GPS_MODE_NOT_VALID 'N' +#define AO_GPS_MODE_AUTONOMOUS 'A' +#define AO_GPS_MODE_DIFFERENTIAL 'D' +#define AO_GPS_MODE_ESTIMATED 'E' +#define AO_GPS_MODE_MANUAL 'M' +#define AO_GPS_MODE_SIMULATED 'S' + +struct ao_telemetry_location { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t flags; /* 5 Number of sats and other flags */ + int16_t altitude; /* 6 GPS reported altitude (m) */ + int32_t latitude; /* 8 latitude (degrees * 10⁷) */ + int32_t longitude; /* 12 longitude (degrees * 10⁷) */ + uint8_t year; /* 16 (- 2000) */ + uint8_t month; /* 17 (1-12) */ + uint8_t day; /* 18 (1-31) */ + uint8_t hour; /* 19 (0-23) */ + uint8_t minute; /* 20 (0-59) */ + uint8_t second; /* 21 (0-59) */ + uint8_t pdop; /* 22 (m * 5) */ + uint8_t hdop; /* 23 (m * 5) */ + uint8_t vdop; /* 24 (m * 5) */ + uint8_t mode; /* 25 */ + uint16_t ground_speed; /* 26 cm/s */ + int16_t climb_rate; /* 28 cm/s */ + uint8_t course; /* 30 degrees / 2 */ + uint8_t unused[1]; /* 31 */ + /* 32 */ +}; + +#define AO_TELEMETRY_SATELLITE 0x06 + +struct ao_telemetry_satellite_info { + uint8_t svid; + uint8_t c_n_1; +}; + +#define AO_TELEMETRY_SATELLITE_MAX_SAT 12 + +struct ao_telemetry_satellite { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + uint8_t channels; /* 5 number of reported sats */ + + struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT]; /* 6 */ + uint8_t unused[2]; /* 30 */ + /* 32 */ +}; + +#define AO_TELEMETRY_COMPANION 0x07 + +#define AO_COMPANION_MAX_CHANNELS 12 + +struct ao_telemetry_companion { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + uint8_t board_id; /* 5 */ + + uint8_t update_period; /* 6 */ + uint8_t channels; /* 7 */ + uint16_t companion_data[AO_COMPANION_MAX_CHANNELS]; /* 8 */ + /* 32 */ +}; + +#define AO_TELEMETRY_MEGA_SENSOR 0x08 + +struct ao_telemetry_mega_sensor { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t orient; /* 5 angle from vertical */ + int16_t accel; /* 6 Z axis */ + + int32_t pres; /* 8 Pa * 10 */ + int16_t temp; /* 12 °C * 100 */ + + int16_t accel_x; /* 14 */ + int16_t accel_y; /* 16 */ + int16_t accel_z; /* 18 */ + + int16_t gyro_x; /* 20 */ + int16_t gyro_y; /* 22 */ + int16_t gyro_z; /* 24 */ + + int16_t mag_x; /* 26 */ + int16_t mag_y; /* 28 */ + int16_t mag_z; /* 30 */ + /* 32 */ +}; + +#define AO_TELEMETRY_MEGA_DATA 0x09 + +struct ao_telemetry_mega_data { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + + int16_t v_batt; /* 6 battery voltage */ + int16_t v_pyro; /* 8 pyro battery voltage */ + int8_t sense[6]; /* 10 continuity sense */ + + int32_t ground_pres; /* 16 average pres on pad */ + int16_t ground_accel; /* 20 average accel on pad */ + int16_t accel_plus_g; /* 22 accel calibration at +1g */ + int16_t accel_minus_g; /* 24 accel calibration at -1g */ + + int16_t acceleration; /* 26 m/s² * 16 */ + int16_t speed; /* 28 m/s * 16 */ + int16_t height; /* 30 m */ + /* 32 */ +}; + + +#define AO_TELEMETRY_METRUM_SENSOR 0x0A + +struct ao_telemetry_metrum_sensor { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + int16_t accel; /* 6 Z axis */ + + int32_t pres; /* 8 Pa * 10 */ + int16_t temp; /* 12 °C * 100 */ + + int16_t acceleration; /* 14 m/s² * 16 */ + int16_t speed; /* 16 m/s * 16 */ + int16_t height; /* 18 m */ + + int16_t v_batt; /* 20 battery voltage */ + int16_t sense_a; /* 22 apogee continuity sense */ + int16_t sense_m; /* 24 main continuity sense */ + + uint8_t pad[6]; /* 26 */ + /* 32 */ +}; + +#define AO_TELEMETRY_METRUM_DATA 0x0B + +struct ao_telemetry_metrum_data { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + int32_t ground_pres; /* 8 average pres on pad */ + int16_t ground_accel; /* 12 average accel on pad */ + int16_t accel_plus_g; /* 14 accel calibration at +1g */ + int16_t accel_minus_g; /* 16 accel calibration at -1g */ + + uint8_t pad[14]; /* 18 */ + /* 32 */ +}; + +#define AO_TELEMETRY_MINI 0x10 + +struct ao_telemetry_mini { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + int16_t v_batt; /* 6 battery voltage */ + int16_t sense_a; /* 8 apogee continuity */ + int16_t sense_m; /* 10 main continuity */ + + int32_t pres; /* 12 Pa * 10 */ + int16_t temp; /* 16 °C * 100 */ + + int16_t acceleration; /* 18 m/s² * 16 */ + int16_t speed; /* 20 m/s * 16 */ + int16_t height; /* 22 m */ + + int32_t ground_pres; /* 24 average pres on pad */ + + int32_t pad28; /* 28 */ + /* 32 */ +}; + +/* #define AO_SEND_ALL_BARO */ + +#define AO_TELEMETRY_BARO 0x80 + +/* + * This packet allows the full sampling rate baro + * data to be captured over the RF link so that the + * flight software can be tested using 'real' data. + * + * Along with this telemetry packet, the flight + * code is modified to send full-rate telemetry all the time + * and never send an RDF tone; this ensure that the full radio + * link is available. + */ +struct ao_telemetry_baro { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + uint8_t samples; /* 5 number samples */ + + int16_t baro[12]; /* 6 samples */ + /* 32 */ +}; + +union ao_telemetry_all { + struct ao_telemetry_generic generic; + struct ao_telemetry_sensor sensor; + struct ao_telemetry_configuration configuration; + struct ao_telemetry_location location; + struct ao_telemetry_satellite satellite; + struct ao_telemetry_companion companion; + struct ao_telemetry_mega_sensor mega_sensor; + struct ao_telemetry_mega_data mega_data; + struct ao_telemetry_metrum_sensor metrum_sensor; + struct ao_telemetry_metrum_data metrum_data; + struct ao_telemetry_mini mini; + struct ao_telemetry_baro baro; +}; + +struct ao_telemetry_all_recv { + union ao_telemetry_all telemetry; + int8_t rssi; + uint8_t status; +}; + +#endif /* _AO_TELEMETRY_H_ */ diff --git a/src/kernel/ao_usb.h b/src/kernel/ao_usb.h new file mode 100644 index 00000000..1ce4f82f --- /dev/null +++ b/src/kernel/ao_usb.h @@ -0,0 +1,142 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_USB_H_ +#define _AO_USB_H_ + +/* + * ao_usb.c + */ + +/* Put one character to the USB output queue */ +void +ao_usb_putchar(char c); + +/* Get one character from the USB input queue */ +char +ao_usb_getchar(void); + +/* Poll for a charcter on the USB input queue. + * returns AO_READ_AGAIN if none are available + */ +int +ao_usb_pollchar(void); + +/* Flush the USB output queue */ +void +ao_usb_flush(void); + +/* Enable the USB controller */ +void +ao_usb_enable(void); + +/* Disable the USB controller */ +void +ao_usb_disable(void); + +/* Initialize the USB system */ +void +ao_usb_init(void); + +extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; + +#define AO_USB_SETUP_DIR_MASK (0x01 << 7) +#define AO_USB_SETUP_TYPE_MASK (0x03 << 5) +#define AO_USB_SETUP_RECIP_MASK (0x1f) + +#define AO_USB_DIR_OUT 0 +#define AO_USB_DIR_IN (1 << 7) + +#define AO_USB_TYPE_STANDARD 0 +#define AO_USB_TYPE_CLASS (1 << 5) +#define AO_USB_TYPE_VENDOR (2 << 5) +#define AO_USB_TYPE_RESERVED (3 << 5) + +#define AO_USB_RECIP_DEVICE 0 +#define AO_USB_RECIP_INTERFACE 1 +#define AO_USB_RECIP_ENDPOINT 2 +#define AO_USB_RECIP_OTHER 3 + +/* standard requests */ +#define AO_USB_REQ_GET_STATUS 0x00 +#define AO_USB_REQ_CLEAR_FEATURE 0x01 +#define AO_USB_REQ_SET_FEATURE 0x03 +#define AO_USB_REQ_SET_ADDRESS 0x05 +#define AO_USB_REQ_GET_DESCRIPTOR 0x06 +#define AO_USB_REQ_SET_DESCRIPTOR 0x07 +#define AO_USB_REQ_GET_CONFIGURATION 0x08 +#define AO_USB_REQ_SET_CONFIGURATION 0x09 +#define AO_USB_REQ_GET_INTERFACE 0x0A +#define AO_USB_REQ_SET_INTERFACE 0x0B +#define AO_USB_REQ_SYNCH_FRAME 0x0C + +#define AO_USB_DESC_DEVICE 1 +#define AO_USB_DESC_CONFIGURATION 2 +#define AO_USB_DESC_STRING 3 +#define AO_USB_DESC_INTERFACE 4 +#define AO_USB_DESC_ENDPOINT 5 +#define AO_USB_DESC_DEVICE_QUALIFIER 6 +#define AO_USB_DESC_OTHER_SPEED 7 +#define AO_USB_DESC_INTERFACE_POWER 8 + +#define AO_USB_GET_DESC_TYPE(x) (((x)>>8)&0xFF) +#define AO_USB_GET_DESC_INDEX(x) ((x)&0xFF) + +#define AO_USB_CONTROL_EP 0 +#define AO_USB_CONTROL_SIZE 32 + +#define AO_USB_INT_EP 1 +#define AO_USB_INT_SIZE 8 + +#ifndef AO_USB_OUT_EP +#define AO_USB_OUT_EP 4 +#define AO_USB_IN_EP 5 +#endif + +/* + * USB bulk packets can only come in 8, 16, 32 and 64 + * byte sizes, so we'll use 64 for everything + */ +#define AO_USB_IN_SIZE 64 +#define AO_USB_OUT_SIZE 64 + +#define AO_USB_EP0_IDLE 0 +#define AO_USB_EP0_DATA_IN 1 +#define AO_USB_EP0_DATA_OUT 2 +#define AO_USB_EP0_STALL 3 + +#define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) + +/* CDC definitions */ +#define AO_USB_CS_INTERFACE 0x24 +#define AO_USB_CS_ENDPOINT 0x25 + +#define AO_USB_SET_LINE_CODING 0x20 +#define AO_USB_GET_LINE_CODING 0x21 +#define AO_USB_SET_CONTROL_LINE_STATE 0x22 + +/* Data structure for GET_LINE_CODING / SET_LINE_CODING class requests */ +struct ao_usb_line_coding { + uint32_t rate; + uint8_t char_format; + uint8_t parity; + uint8_t data_bits; +} ; + +extern __pdata uint8_t ao_usb_running; + +#endif /* _AO_USB_H_ */ diff --git a/src/lpc/Makefile-lpc.defs b/src/lpc/Makefile-lpc.defs index fecb9135..bccea5bc 100644 --- a/src/lpc/Makefile-lpc.defs +++ b/src/lpc/Makefile-lpc.defs @@ -4,7 +4,7 @@ endif include $(TOPDIR)/Makedefs -vpath % $(TOPDIR)/lpc:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/core:$(TOPDIR)/util:$(TOPDIR)/kalman:$(TOPDIR/aes):$(TOPDIR):$(TOPDIR)/math +vpath % $(TOPDIR)/lpc:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/kernel:$(TOPDIR)/util:$(TOPDIR)/kalman:$(TOPDIR/aes):$(TOPDIR):$(TOPDIR)/math vpath make-altitude $(TOPDIR)/util vpath make-kalman $(TOPDIR)/util vpath kalman.5c $(TOPDIR)/kalman @@ -28,7 +28,7 @@ CC=$(ARM_CC) WARN_FLAGS=-Wall -Wextra -Werror -AO_CFLAGS=-I. -I$(TOPDIR)/lpc -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) -I$(TOPDIR)/math -I$(TOPDIR) $(PDCLIB_INCLUDES) +AO_CFLAGS=-I. -I$(TOPDIR)/lpc -I$(TOPDIR)/kernel -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) -I$(TOPDIR)/math -I$(TOPDIR) $(PDCLIB_INCLUDES) LPC_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m0 -mthumb\ -ffreestanding -nostdlib $(AO_CFLAGS) $(WARN_FLAGS) diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index dcc32874..f4c57076 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -2,7 +2,7 @@ # Tiny AltOS build # # -vpath % ../attiny:../drivers:../core:../product:.. +vpath % ../attiny:../drivers:../kernel:../product:.. vpath ao-make-product.5c ../util vpath make-altitude-pa ../util @@ -47,7 +47,7 @@ INC=\ IDPRODUCT=0 PRODUCT=MicroPeak-v0.1 PRODUCT_DEF=-DMICROPEAK -CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -I../product +CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../kernel -I.. -I../drivers -I../product CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O2 -mcall-prologues -DATTINY NICKLE=nickle diff --git a/src/nanopeak-v0.1/Makefile b/src/nanopeak-v0.1/Makefile index 04eea902..d3779594 100644 --- a/src/nanopeak-v0.1/Makefile +++ b/src/nanopeak-v0.1/Makefile @@ -2,7 +2,7 @@ # Tiny AltOS build # # -vpath % ../attiny:../drivers:../core:../product:.. +vpath % ../attiny:../drivers:../kernel:../product:.. vpath ao-make-product.5c ../util vpath make-altitude-pa ../util @@ -47,7 +47,7 @@ INC=\ IDPRODUCT=0 PRODUCT=NanoPeak-v0.1 PRODUCT_DEF=-DNANOPEAK -CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -I../product +CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../kernel -I.. -I../drivers -I../product CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O2 -mcall-prologues -DATTINY NICKLE=nickle diff --git a/src/product/Makefile.teledongle b/src/product/Makefile.teledongle index da9bcba0..81151364 100644 --- a/src/product/Makefile.teledongle +++ b/src/product/Makefile.teledongle @@ -7,8 +7,8 @@ # TD_VER, TD_DEF and include # this file -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/product/Makefile.telelaunch b/src/product/Makefile.telelaunch index a5e2eb7f..90fe7833 100644 --- a/src/product/Makefile.telelaunch +++ b/src/product/Makefile.telelaunch @@ -4,8 +4,8 @@ # define TELELAUNCH_VER, TELELAUNCH_DEF # this file -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/product/Makefile.telemetrum b/src/product/Makefile.telemetrum index c740a483..dbbf57d8 100644 --- a/src/product/Makefile.telemetrum +++ b/src/product/Makefile.telemetrum @@ -7,8 +7,8 @@ # TM_VER, TM_DEF, TM_INC and TM_SRC and include # this file -vpath %.c .:..:../core:../cc1111:../drivers:../product -vpath %.h .:..:../core:../cc1111:../drivers:../product +vpath %.c .:..:../kernel:../cc1111:../drivers:../product +vpath %.h .:..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/product/Makefile.telemini b/src/product/Makefile.telemini index 0884079e..ff8b9d56 100644 --- a/src/product/Makefile.telemini +++ b/src/product/Makefile.telemini @@ -4,8 +4,8 @@ # Define TELEMINI_VER and TELEMINI_DEF and then # include this file -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/product/Makefile.telenano b/src/product/Makefile.telenano index c31989ee..d2fcb6d8 100644 --- a/src/product/Makefile.telenano +++ b/src/product/Makefile.telenano @@ -4,8 +4,8 @@ # Define TELENANO_VER and TELENANO_DEF and then # include this file -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/spiradio-v0.1/.sdcdbrc b/src/spiradio-v0.1/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/spiradio-v0.1/.sdcdbrc +++ b/src/spiradio-v0.1/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/spiradio-v0.1/Makefile b/src/spiradio-v0.1/Makefile index e644bc49..cd7a9cde 100644 --- a/src/spiradio-v0.1/Makefile +++ b/src/spiradio-v0.1/Makefile @@ -5,8 +5,8 @@ SPIRADIO_VER=0.1 SPIRADIO_DEF=0_1 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/stm-bringup/Makefile b/src/stm-bringup/Makefile index 797df2d6..b0943e56 100644 --- a/src/stm-bringup/Makefile +++ b/src/stm-bringup/Makefile @@ -1,4 +1,4 @@ -vpath % ..:../core:../product:../drivers:../stm +vpath % ..:../kernel:../product:../drivers:../stm vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/stm/Makefile-flash.defs b/src/stm/Makefile-flash.defs index 3890eff1..dde51a68 100644 --- a/src/stm/Makefile-flash.defs +++ b/src/stm/Makefile-flash.defs @@ -1,4 +1,4 @@ -vpath % $(TOPDIR)/stm:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/core:$(TOPDIR)/util:$(TOPDIR) +vpath % $(TOPDIR)/stm:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/kernel:$(TOPDIR)/util:$(TOPDIR) vpath ao-make-product.5c $(TOPDIR)/util .SUFFIXES: .elf .ihx @@ -14,7 +14,7 @@ include $(TOPDIR)/Makedefs CC=$(ARM_CC) LIBS=$(PDCLIB_LIBS_M3) -lgcc -AO_CFLAGS=-I. -I$(TOPDIR)/stm -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES) +AO_CFLAGS=-I. -I$(TOPDIR)/stm -I$(TOPDIR)/kernel -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES) STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) $(SAT_CFLAGS) LDFLAGS=-L$(TOPDIR)/stm -Wl,-Taltos-loader.ld diff --git a/src/stm/Makefile.defs b/src/stm/Makefile.defs index 42adfd09..c3d2707f 100644 --- a/src/stm/Makefile.defs +++ b/src/stm/Makefile.defs @@ -1,4 +1,4 @@ -vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:../math:.. +vpath % ../stm:../product:../drivers:../kernel:../util:../kalman:../aes:../math:.. vpath make-altitude ../util vpath make-kalman ../util vpath kalman.5c ../kalman @@ -26,7 +26,7 @@ LIBS=$(PDCLIB_LIBS_M3) -lgcc WARN_FLAGS=-Wall -Wextra -Werror -AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I../math -I.. $(PDCLIB_INCLUDES) +AO_CFLAGS=-I. -I../stm -I../kernel -I../drivers -I../math -I.. $(PDCLIB_INCLUDES) STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb \ -ffreestanding -nostdlib $(AO_CFLAGS) $(WARN_FLAGS) diff --git a/src/teleballoon-v1.1/Makefile b/src/teleballoon-v1.1/Makefile index 6ff076a9..c6f6345a 100644 --- a/src/teleballoon-v1.1/Makefile +++ b/src/teleballoon-v1.1/Makefile @@ -17,8 +17,8 @@ TELEBALLOON_SRC = \ ao_gps_skytraq.c \ ao_m25.c -vpath %.c ..:../core:../cc1111:../drivers:../product:. -vpath %.h ..:../core:../cc1111:../drivers:../product:. +vpath %.c ..:../kernel:../cc1111:../drivers:../product:. +vpath %.h ..:../kernel:../cc1111:../drivers:../product:. vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telebt-v1.0/.sdcdbrc b/src/telebt-v1.0/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telebt-v1.0/.sdcdbrc +++ b/src/telebt-v1.0/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telebt-v1.0/Makefile b/src/telebt-v1.0/Makefile index 40853fc3..a7797499 100644 --- a/src/telebt-v1.0/Makefile +++ b/src/telebt-v1.0/Makefile @@ -5,8 +5,8 @@ TELEBT_VER=1.0 TELEBT_DEF=1_0 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telefire-v0.1/.sdcdbrc b/src/telefire-v0.1/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telefire-v0.1/.sdcdbrc +++ b/src/telefire-v0.1/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telefire-v0.1/Makefile b/src/telefire-v0.1/Makefile index f9e11698..25267268 100644 --- a/src/telefire-v0.1/Makefile +++ b/src/telefire-v0.1/Makefile @@ -5,8 +5,8 @@ TELEFIRE_VER=0.1 TELEFIRE_DEF=0_1 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telefire-v0.2/.sdcdbrc b/src/telefire-v0.2/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telefire-v0.2/.sdcdbrc +++ b/src/telefire-v0.2/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile index a820990a..ad5065c1 100644 --- a/src/telefire-v0.2/Makefile +++ b/src/telefire-v0.2/Makefile @@ -5,8 +5,8 @@ TELEFIRE_VER=0.2 TELEFIRE_DEF=0_2 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telemetrum-v1.0/.sdcdbrc b/src/telemetrum-v1.0/.sdcdbrc index fbe9a599..e9a51ea6 100644 --- a/src/telemetrum-v1.0/.sdcdbrc +++ b/src/telemetrum-v1.0/.sdcdbrc @@ -1 +1 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telemetrum-v1.1/.sdcdbrc b/src/telemetrum-v1.1/.sdcdbrc index fbe9a599..e9a51ea6 100644 --- a/src/telemetrum-v1.1/.sdcdbrc +++ b/src/telemetrum-v1.1/.sdcdbrc @@ -1 +1 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telemini-v1.0/.sdcdbrc b/src/telemini-v1.0/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telemini-v1.0/.sdcdbrc +++ b/src/telemini-v1.0/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telemini-v2.0/.sdcdbrc b/src/telemini-v2.0/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telemini-v2.0/.sdcdbrc +++ b/src/telemini-v2.0/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile index fcac2c48..606b4f3b 100644 --- a/src/telemini-v2.0/Makefile +++ b/src/telemini-v2.0/Makefile @@ -5,8 +5,8 @@ TELEMINI_VER=2.0 TELEMINI_DEF=2_0 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telepyro-v0.1/Makefile b/src/telepyro-v0.1/Makefile index 025b324a..dcac03dc 100644 --- a/src/telepyro-v0.1/Makefile +++ b/src/telepyro-v0.1/Makefile @@ -2,7 +2,7 @@ # AltOS build # # -vpath % .:..:../core:../product:../drivers:../avr +vpath % .:..:../kernel:../product:../drivers:../avr vpath ao-make-product.5c ../util include ../avr/Makefile.defs @@ -45,7 +45,7 @@ PRODUCT=TelePyro-v0.1 MCU=atmega32u4 PRODUCT_DEF=-DTELEPYRO IDPRODUCT=0x0011 -CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../core -I.. +CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../kernel -I.. CFLAGS += -mmcu=$(MCU) -Wall -Wstrict-prototypes -O3 -mcall-prologues -DAVR NICKLE=nickle diff --git a/src/telescience-pwm/Makefile b/src/telescience-pwm/Makefile index de81b8d7..493bd480 100644 --- a/src/telescience-pwm/Makefile +++ b/src/telescience-pwm/Makefile @@ -2,7 +2,7 @@ # AltOS build # # -vpath % ..:../core:../product:../drivers:../avr +vpath % ..:../kernel:../product:../drivers:../avr vpath ao-make-product.5c ../util include ../avr/Makefile.defs @@ -57,7 +57,7 @@ PRODUCT=TeleScience-PWM MCU=atmega32u4 PRODUCT_DEF=-DTELESCIENCE -DTELESCIENCE_PWM IDPRODUCT=0x0011 -CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../core -I.. +CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../kernel -I.. CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O3 -mcall-prologues -DAVR NICKLE=nickle diff --git a/src/telescience-v0.1/Makefile b/src/telescience-v0.1/Makefile index 6e4eb6de..c55c48e2 100644 --- a/src/telescience-v0.1/Makefile +++ b/src/telescience-v0.1/Makefile @@ -2,7 +2,7 @@ # AltOS build # # -vpath % ..:../core:../product:../drivers:../avr +vpath % ..:../kernel:../product:../drivers:../avr vpath ao-make-product.5c ../util include ../avr/Makefile.defs @@ -56,7 +56,7 @@ PRODUCT=TeleScience-v0.1 MCU=atmega32u4 PRODUCT_DEF=-DTELESCIENCE IDPRODUCT=0x0011 -CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../core -I.. +CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../kernel -I.. CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O3 -mcall-prologues -DAVR NICKLE=nickle diff --git a/src/teleshield-v0.1/Makefile b/src/teleshield-v0.1/Makefile index e8b262ef..f54488a2 100644 --- a/src/teleshield-v0.1/Makefile +++ b/src/teleshield-v0.1/Makefile @@ -17,8 +17,8 @@ TELESHIELD_SRC = \ ao_btm.c \ ao_spi.c -vpath %.c ..:../core:../cc1111:../drivers:../product:. -vpath %.h ..:../core:../cc1111:../drivers:../product:. +vpath %.c ..:../kernel:../cc1111:../drivers:../product:. +vpath %.h ..:../kernel:../cc1111:../drivers:../product:. vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/teleterra-v0.2/.sdcdbrc b/src/teleterra-v0.2/.sdcdbrc index fbe9a599..e9a51ea6 100644 --- a/src/teleterra-v0.2/.sdcdbrc +++ b/src/teleterra-v0.2/.sdcdbrc @@ -1 +1 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/teleterra-v0.2/Makefile b/src/teleterra-v0.2/Makefile index 88637360..826c52e5 100644 --- a/src/teleterra-v0.2/Makefile +++ b/src/teleterra-v0.2/Makefile @@ -2,8 +2,8 @@ # TeleTerra build file # -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/test/Makefile b/src/test/Makefile index c6025219..7cb86140 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,4 +1,4 @@ -vpath % ..:../core:../drivers:../util:../micropeak:../aes:../product +vpath % ..:../kernel:../drivers:../util:../micropeak:../aes:../product PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ @@ -9,7 +9,7 @@ INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h ao_quat KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -I../product -O0 -g -Wall +CFLAGS=-I.. -I. -I../kernel -I../drivers -I../micropeak -I../product -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -52,7 +52,7 @@ ao_kalman.h: $(KALMAN) (cd .. && make ao_kalman.h) ao_fec_test: ao_fec_test.c ao_fec_tx.c ao_fec_rx.c - cc $(CFLAGS) -DAO_FEC_DEBUG=1 -o $@ ao_fec_test.c ../core/ao_fec_tx.c ../core/ao_fec_rx.c -lm + cc $(CFLAGS) -DAO_FEC_DEBUG=1 -o $@ ao_fec_test.c ../kernel/ao_fec_tx.c ../kernel/ao_fec_rx.c -lm ao_aprs_test: ao_aprs_test.c ao_aprs.c cc $(CFLAGS) -o $@ ao_aprs_test.c diff --git a/src/tidongle/Makefile b/src/tidongle/Makefile index b2ba537b..0e90d744 100644 --- a/src/tidongle/Makefile +++ b/src/tidongle/Makefile @@ -2,8 +2,8 @@ # TIDongle build file # -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION