diff options
Diffstat (limited to 'src/core')
37 files changed, 1587 insertions, 220 deletions
diff --git a/src/core/ao.h b/src/core/ao.h index 31ec4686..0ad3e4aa 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -39,64 +39,15 @@ #define CODE_TO_XDATA(a) (a) #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 */ - uint8_t stack[AO_STACK_SIZE]; /* saved stack */ -}; - -extern __xdata struct ao_task *__data ao_cur_task; - -#define AO_NUM_TASKS 16 /* maximum number of tasks */ -#define AO_NO_TASK 0 /* no task id */ - -/* - 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); - -/* 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; - -/* Terminate the current task */ -void -ao_exit(void); - -/* Dump task info to console */ -void -ao_task_info(void); +#ifndef HAS_TASK +#define HAS_TASK 1 +#endif -/* Start the scheduler. This will not return */ -void -ao_start_scheduler(void); +#if HAS_TASK +#include <ao_task.h> +#else +#include <ao_notask.h> +#endif /* * ao_panic.c @@ -115,6 +66,8 @@ ao_start_scheduler(void); #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_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 */ @@ -136,12 +89,14 @@ ao_panic(uint8_t reason); 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 */ -uint16_t +AO_TICK_TYPE ao_time(void); /* Suspend the current task until ticks time has passed */ @@ -150,7 +105,7 @@ ao_delay(uint16_t ticks); /* Set the ADC interval */ void -ao_timer_set_adc_interval(uint8_t interval) __critical; +ao_timer_set_adc_interval(uint8_t interval); /* Timer interrupt */ void @@ -168,11 +123,13 @@ 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 @@ -190,6 +147,9 @@ 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 @@ -216,6 +176,10 @@ ao_cmd_hex(void); void ao_cmd_decimal(void); +/* Read a single hex nibble off stdin. */ +uint8_t +ao_getnibble(void); + uint8_t ao_match_word(__code char *word); @@ -319,11 +283,13 @@ ao_temp_to_dC(int16_t temp) __reentrant; * Convert between pressure in Pa and altitude in meters */ -int32_t +#include <ao_data.h> + +alt_t ao_pa_to_altitude(int32_t pa); int32_t -ao_altitude_to_pa(int32_t alt); +ao_altitude_to_pa(alt_t alt); #if HAS_DBG #include <ao_dbg.h> @@ -339,10 +305,10 @@ ao_altitude_to_pa(int32_t alt); */ uint8_t -ao_spi_slave_recv(uint8_t *buf, uint8_t len); +ao_spi_slave_recv(void *buf, uint16_t len); void -ao_spi_slave_send(uint8_t *buf, uint8_t len); +ao_spi_slave_send(void *buf, uint16_t len); void ao_spi_slave_init(void); @@ -547,6 +513,8 @@ ao_telemetry_tiny_init(void); 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 @@ -554,21 +522,56 @@ extern __xdata uint8_t ao_radio_dma; #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) __reentrant; +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: * @@ -602,8 +605,10 @@ extern const char const * const ao_state_names[]; 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]; @@ -637,10 +642,10 @@ ao_monitor_init(void) __reentrant; * ao_stdio.c */ -#define AO_READ_AGAIN ((char) -1) +#define AO_READ_AGAIN (-1) struct ao_stdio { - char (*pollchar)(void); + int (*_pollchar)(void); /* Called with interrupts blocked */ void (*putchar)(char c) __reentrant; void (*flush)(void); uint8_t echo; @@ -659,7 +664,7 @@ uint8_t ao_echo(void); int8_t -ao_add_stdio(char (*pollchar)(void), +ao_add_stdio(int (*pollchar)(void), void (*putchar)(char) __reentrant, void (*flush)(void)) __reentrant; @@ -717,7 +722,7 @@ extern __xdata uint8_t ao_force_freq; #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 12 +#define AO_CONFIG_MINOR 14 #define AO_AES_LEN 16 @@ -744,12 +749,23 @@ struct ao_config { #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 }; #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 diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c index 1814cecf..188b8bb4 100644 --- a/src/core/ao_cmd.c +++ b/src/core/ao_cmd.c @@ -16,6 +16,7 @@ */ #include "ao.h" +#include "ao_task.h" __pdata uint16_t ao_cmd_lex_i; __pdata uint32_t ao_cmd_lex_u32; @@ -28,8 +29,8 @@ static __xdata char cmd_line[CMD_LEN]; static __pdata uint8_t cmd_len; static __pdata uint8_t cmd_i; -static void -put_string(__code char *s) +void +ao_put_string(__code char *s) { char c; while ((c = *s++)) @@ -39,7 +40,7 @@ put_string(__code char *s) static void backspace(void) { - put_string ("\010 \010"); + ao_put_string ("\010 \010"); } static void @@ -47,7 +48,7 @@ readline(void) { char c; if (ao_echo()) - put_string("> "); + ao_put_string("> "); cmd_len = 0; for (;;) { flush(); @@ -110,6 +111,22 @@ putnibble(uint8_t v) putchar(v + ('a' - 10)); } +uint8_t +ao_getnibble(void) +{ + char c; + + c = getchar(); + if ('0' <= c && c <= '9') + return c - '0'; + if ('a' <= c && c <= 'f') + return c - ('a' - 10); + if ('A' <= c && c <= 'F') + return c - ('A' - 10); + ao_cmd_status = ao_cmd_lex_error; + return 0; +} + void ao_cmd_put16(uint16_t v) { @@ -246,20 +263,39 @@ ao_reboot(void) ao_panic(AO_PANIC_REBOOT); } +#ifndef HAS_VERSION +#define HAS_VERSION 1 +#endif + +#if HAS_VERSION static void version(void) { - printf("manufacturer %s\n", ao_manufacturer); - printf("product %s\n", ao_product); - printf("serial-number %u\n", ao_serial_number); + printf("manufacturer %s\n" + "product %s\n" + "serial-number %u\n" +#if HAS_FLIGHT + "current-flight %u\n" +#endif #if HAS_LOG - printf("log-format %u\n", ao_log_format); + "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 + ); #if HAS_MS5607 ao_ms5607_info(); #endif printf("software-version %s\n", ao_version); } +#endif #ifndef NUM_CMDS #define NUM_CMDS 11 @@ -274,13 +310,21 @@ help(void) __pdata uint8_t cmds; __pdata uint8_t cmd; __code struct ao_cmds * __pdata cs; - const char *h; + __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; - printf("%-45s %s\n", h, h + 1 + strlen(h)); + ao_put_string(h); + e = strlen(h); + h += e + 1; + e = 45 - e; + while (e--) + putchar(' '); + ao_put_string(h); + putchar('\n'); } } } @@ -341,14 +385,33 @@ ao_cmd(void) } } +#if HAS_BOOT_LOADER + +#include <ao_boot.h> + +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 }, }; diff --git a/src/core/ao_config.c b/src/core/ao_config.c index ce855ad1..73608a55 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -47,6 +47,8 @@ __xdata uint8_t ao_config_mutex; #define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 192 * (uint32_t) 1024) #endif #endif +#define AO_CONFIG_DEFAULT_RADIO_POWER 0x60 +#define AO_CONFIG_DEFAULT_RADIO_AMP 0 #if HAS_EEPROM static void @@ -102,6 +104,7 @@ _ao_config_get(void) 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) { @@ -127,24 +130,38 @@ _ao_config_get(void) if (minor < 6) ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION; if (minor < 8) - ao_config.radio_enable = TRUE; + 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.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 = 0; +#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 ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; } #if HAS_RADIO #if HAS_FORCE_FREQ - if (ao_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 @@ -203,6 +220,7 @@ ao_config_callsign_set(void) __reentrant } #if HAS_RADIO + void ao_config_frequency_show(void) __reentrant { @@ -220,7 +238,9 @@ ao_config_frequency_set(void) __reentrant 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 @@ -493,6 +513,69 @@ ao_config_key_set(void) __reentrant } #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; @@ -524,15 +607,23 @@ __code struct ao_config_var ao_config_vars[] = { ao_config_callsign_set, ao_config_callsign_show }, { "e <0 disable, 1 enable>\0Enable telemetry and RDF", ao_config_radio_enable_set, ao_config_radio_enable_show }, + { "f <cal>\0Radio calib (cal = rf/(xtal/2^16))", + ao_config_radio_cal_set, ao_config_radio_cal_show }, +#if HAS_RADIO_POWER + { "p <setting>\0Radio power setting (0-255)", + ao_config_radio_power_set, ao_config_radio_power_show }, +#endif +#if HAS_RADIO_AMP + { "d <setting>\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_RADIO - { "f <cal>\0Radio calib (cal = rf/(xtal/2^16))", - ao_config_radio_cal_set, ao_config_radio_cal_show }, -#endif /* HAS_RADIO */ #if HAS_LOG { "l <size>\0Flight log size (kB)", ao_config_log_set, ao_config_log_show }, @@ -541,10 +632,6 @@ __code struct ao_config_var ao_config_vars[] = { { "i <0 dual, 1 apogee, 2 main>\0Set igniter mode", ao_config_ignite_mode_set, ao_config_ignite_mode_show }, #endif -#if HAS_ACCEL - { "o <0 antenna up, 1 antenna down>\0Set pad orientation", - ao_config_pad_orientation_set,ao_config_pad_orientation_show }, -#endif #if HAS_AES { "k <32 hex digits>\0Set AES encryption key", ao_config_key_set, ao_config_key_show }, @@ -553,6 +640,10 @@ __code struct ao_config_var ao_config_vars[] = { { "P <n,?>\0Configure pyro channels", ao_pyro_set, ao_pyro_show }, #endif +#if HAS_APRS + { "A <secs>\0APRS packet interval (0 disable)", + ao_config_aprs_set, ao_config_aprs_show }, +#endif { "s\0Show", ao_config_show, 0 }, #if HAS_EEPROM diff --git a/src/core/ao_convert_pa.c b/src/core/ao_convert_pa.c index 0c93caea..fe6e0ef6 100644 --- a/src/core/ao_convert_pa.c +++ b/src/core/ao_convert_pa.c @@ -19,14 +19,22 @@ #include "ao.h" #endif -static const int32_t altitude_table[] = { +#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) -int32_t +alt_t ao_pa_to_altitude(int32_t pa) { int16_t o; @@ -35,16 +43,17 @@ ao_pa_to_altitude(int32_t pa) if (pa < 0) pa = 0; - if (pa > 120000) - pa = 120000; + if (pa > 120000L) + pa = 120000L; o = pa >> ALT_SHIFT; part = pa & ALT_MASK; - low = (int32_t) altitude_table[o] * (ALT_SCALE - part); - high = (int32_t) altitude_table[o+1] * part + (ALT_SCALE >> 1); + 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) { @@ -70,3 +79,4 @@ ao_altitude_to_pa(int32_t alt) pa = 0; return pa; } +#endif diff --git a/src/core/ao_convert_pa_test.c b/src/core/ao_convert_pa_test.c index 972a4d4c..7d5b1922 100644 --- a/src/core/ao_convert_pa_test.c +++ b/src/core/ao_convert_pa_test.c @@ -17,15 +17,17 @@ #include <stdint.h> #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 i_abs(int i) { return i < 0 ? -i : i; } +static inline int i_abs(int i) { return i < 0 ? -i : i; } -main () +int +main (int argc, char **argv) { int i; int32_t p_to_a, p_to_a_to_p; @@ -49,9 +51,7 @@ main () // printf ("pa %d alt %d pa %d\n", // i, p_to_a, p_to_a_to_p); } - for (i = -1450; i < 74250 + STEP_A; i += STEP_A) { - if (i > 74250) - i = 74250; + 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); diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 2b9ef5ac..7e2f85d8 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -52,6 +52,8 @@ #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 { @@ -65,6 +67,9 @@ struct ao_data { #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; @@ -85,14 +90,7 @@ extern volatile __data uint8_t ao_data_count; /* * Mark a section of data as ready, check for data complete */ -#define AO_DATA_PRESENT(bit) do { \ - if ((ao_data_present |= (bit)) == AO_DATA_ALL) { \ - ao_data_ring[ao_data_head].tick = ao_tick_count; \ - ao_data_head = ao_data_ring_next(ao_data_head); \ - ao_data_present = 0; \ - ao_wakeup((void *) &ao_data_head); \ - } \ - } while (0); +#define AO_DATA_PRESENT(bit) (ao_data_present |= (bit)) /* * Wait until it is time to write a sensor sample; this is @@ -102,6 +100,8 @@ extern volatile __data uint8_t ao_data_count; ao_sleep((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 @@ -110,7 +110,12 @@ extern volatile __data uint8_t ao_data_count; #define HAS_BARO 1 typedef int32_t pres_t; -typedef int32_t alt_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) @@ -135,6 +140,10 @@ typedef int16_t alt_t; #endif +#if !HAS_BARO +typedef int16_t alt_t; +#endif + /* * Need a few macros to pull data from the sensors: * @@ -272,11 +281,32 @@ typedef int16_t accel_t; typedef int16_t accel_t; /* MPU6000 is hooked up so that positive y is positive acceleration */ -#define ao_data_accel(packet) ((packet)->mpu6000.accel_y) +#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)->mpu6000.accel_y = (accel)) +#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; +typedef int32_t angle_t; + +/* 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 + #endif /* _AO_DATA_H_ */ diff --git a/src/core/ao_fec_rx.c b/src/core/ao_fec_rx.c index 072a9e90..c4f5559a 100644 --- a/src/core/ao_fec_rx.c +++ b/src/core/ao_fec_rx.c @@ -18,7 +18,7 @@ #include <ao_fec.h> #include <stdio.h> -#ifdef MEGAMETRUM +#ifdef TELEMEGA #include <ao.h> #endif diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index aa4f6961..9d9d4c6e 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -94,7 +94,9 @@ ao_flight(void) 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_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 @@ -115,7 +117,7 @@ ao_flight(void) { /* Set pad mode - we can fly! */ ao_flight_state = ao_flight_pad; -#if HAS_USB && HAS_RADIO && !HAS_FLIGHT_DEBUG +#if HAS_USB && HAS_RADIO && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE /* Disable the USB controller in flight mode * to save power */ diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c index 47891cab..e3af4307 100644 --- a/src/core/ao_gps_report_mega.c +++ b/src/core/ao_gps_report_mega.c @@ -16,6 +16,7 @@ */ #include "ao.h" +#include "ao_log.h" void ao_gps_report_mega(void) diff --git a/src/core/ao_ignite.c b/src/core/ao_ignite.c index c7829fc3..74bd0c5a 100644 --- a/src/core/ao_ignite.c +++ b/src/core/ao_ignite.c @@ -21,10 +21,12 @@ __xdata struct ao_ignition ao_ignition[2]; void -ao_ignite(enum ao_igniter igniter) __critical +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 @@ -39,12 +41,12 @@ ao_igniter_status(enum ao_igniter igniter) __pdata int16_t value; __pdata uint8_t request, firing, fired; - __critical { + 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; @@ -79,7 +81,7 @@ ao_igniter_status(enum ao_igniter igniter) #endif void -ao_igniter_fire(enum ao_igniter igniter) __critical +ao_igniter_fire(enum ao_igniter igniter) { ao_ignition[igniter].firing = 1; switch(ao_config.ignite_mode) { diff --git a/src/core/ao_list.h b/src/core/ao_list.h new file mode 100644 index 00000000..8a6fa4d9 --- /dev/null +++ b/src/core/ao_list.h @@ -0,0 +1,213 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public 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 <stddef.h> + +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.h b/src/core/ao_log.h index 04abeb7e..a68a40dd 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -43,7 +43,7 @@ extern __pdata enum ao_flight_state ao_log_state; #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_MEGAMETRUM 5 /* 32 byte typed megametrum records */ +#define AO_LOG_FORMAT_TELEMEGA 5 /* 32 byte typed telemega records */ #define AO_LOG_FORMAT_NONE 127 /* No log at all */ extern __code uint8_t ao_log_format; @@ -138,17 +138,17 @@ ao_log_full(void); #define AO_LOG_POS_NONE (~0UL) struct ao_log_record { - char type; - uint8_t csum; - uint16_t tick; + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ union { struct { - int16_t ground_accel; - uint16_t flight; + int16_t ground_accel; /* 4 */ + uint16_t flight; /* 6 */ } flight; struct { - int16_t accel; - int16_t pres; + int16_t accel; /* 4 */ + int16_t pres; /* 6 */ } sensor; struct { int16_t temp; @@ -197,16 +197,24 @@ struct ao_log_mega { 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 */ + 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 */ @@ -221,12 +229,14 @@ struct ao_log_mega { 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 */ } volt; /* 30 */ + /* AO_LOG_GPS_TIME */ struct { int32_t latitude; /* 4 */ int32_t longitude; /* 8 */ @@ -240,6 +250,7 @@ struct ao_log_mega { uint8_t day; /* 20 */ uint8_t pad; /* 21 */ } gps; /* 22 */ + /* AO_LOG_GPS_SAT */ struct { uint16_t channels; /* 4 */ struct { @@ -257,4 +268,7 @@ ao_log_data(__xdata struct ao_log_record *log) __reentrant; uint8_t ao_log_mega(__xdata struct ao_log_mega *log) __reentrant; +void +ao_log_flush(void); + #endif /* _AO_LOG_H_ */ diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c index ac1590db..abf953a6 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -23,7 +23,7 @@ static __xdata uint8_t ao_log_mutex; static __xdata struct ao_log_mega log; -__code uint8_t ao_log_format = AO_LOG_FORMAT_MEGAMETRUM; +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMEGA; static uint8_t ao_log_csum(__xdata uint8_t *b) __reentrant @@ -95,6 +95,14 @@ ao_log(void) #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); @@ -163,6 +171,8 @@ ao_log(void) } #endif + ao_log_flush(); + /* Wait for a while */ ao_delay(AO_MS_TO_TICKS(100)); diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c index 18ab85dd..23ebf7dd 100644 --- a/src/core/ao_log_telem.c +++ b/src/core/ao_log_telem.c @@ -102,9 +102,9 @@ ao_log_single(void) while (ao_log_running) { /* Write samples to EEPROM */ while (ao_log_monitor_pos != ao_monitor_head) { - memcpy(&ao_log_single_write_data.telemetry, - &ao_monitor_ring[ao_log_monitor_pos], - AO_LOG_SINGLE_SIZE); + ao_xmemcpy(&ao_log_single_write_data.telemetry, + &ao_monitor_ring[ao_log_monitor_pos], + AO_LOG_SINGLE_SIZE); ao_log_single_write(); ao_log_monitor_pos = ao_monitor_ring_next(ao_log_monitor_pos); ao_log_telem_track(); diff --git a/src/core/ao_monitor.c b/src/core/ao_monitor.c index 5876bef7..18f170b4 100644 --- a/src/core/ao_monitor.c +++ b/src/core/ao_monitor.c @@ -81,7 +81,7 @@ ao_monitor_get(void) size = ao_monitoring; break; } - if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2)) + 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)); diff --git a/src/core/ao_mutex.c b/src/core/ao_mutex.c index c82a7d57..952ff462 100644 --- a/src/core/ao_mutex.c +++ b/src/core/ao_mutex.c @@ -22,11 +22,11 @@ ao_mutex_get(__xdata uint8_t *mutex) __reentrant { if (*mutex == ao_cur_task->task_id) ao_panic(AO_PANIC_MUTEX); - __critical { + ao_arch_critical( while (*mutex) ao_sleep(mutex); *mutex = ao_cur_task->task_id; - } + ); } void @@ -34,8 +34,8 @@ ao_mutex_put(__xdata uint8_t *mutex) __reentrant { if (*mutex != ao_cur_task->task_id) ao_panic(AO_PANIC_MUTEX); - __critical { + ao_arch_critical( *mutex = 0; ao_wakeup(mutex); - } + ); } diff --git a/src/core/ao_notask.c b/src/core/ao_notask.c new file mode 100644 index 00000000..a41712d2 --- /dev/null +++ b/src/core/ao_notask.c @@ -0,0 +1,45 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public 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 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) +{ + ao_wchan = 0; +} diff --git a/src/core/ao_notask.h b/src/core/ao_notask.h new file mode 100644 index 00000000..6b6b5bb8 --- /dev/null +++ b/src/core/ao_notask.h @@ -0,0 +1,27 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public 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 index f232a878..b8426cf9 100644 --- a/src/core/ao_packet.h +++ b/src/core/ao_packet.h @@ -48,6 +48,7 @@ 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); @@ -61,13 +62,13 @@ ao_packet_flush(void); void ao_packet_putchar(char c) __reentrant; -char -ao_packet_pollchar(void) __critical; +int +_ao_packet_pollchar(void); #if PACKET_HAS_MASTER /* ao_packet_master.c */ -extern __xdata uint8_t ao_packet_last_rssi; +extern __xdata int8_t ao_packet_last_rssi; void ao_packet_master_init(void); diff --git a/src/core/ao_panic.c b/src/core/ao_panic.c index 52433044..c29cd8fe 100644 --- a/src/core/ao_panic.c +++ b/src/core/ao_panic.c @@ -29,6 +29,10 @@ #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) { @@ -49,13 +53,14 @@ ao_panic(uint8_t reason) ao_cur_task = NULL; printf ("panic %d\n", reason); #endif - __critical for (;;) { + ao_arch_block_interrupts(); + for (;;) { ao_panic_delay(20); for (n = 0; n < 5; n++) { - ao_led_on(AO_LED_RED); + ao_led_on(AO_LED_PANIC); ao_beep(AO_BEEP_HIGH); ao_panic_delay(1); - ao_led_off(AO_LED_RED); + ao_led_off(AO_LED_PANIC); ao_beep(AO_BEEP_LOW); ao_panic_delay(1); } @@ -66,18 +71,18 @@ ao_panic(uint8_t reason) #pragma disable_warning 126 #endif if (reason & 0x40) { - ao_led_on(AO_LED_RED); + ao_led_on(AO_LED_PANIC); ao_beep(AO_BEEP_HIGH); ao_panic_delay(40); - ao_led_off(AO_LED_RED); + 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_RED); + ao_led_on(AO_LED_PANIC); ao_beep(AO_BEEP_MID); ao_panic_delay(10); - ao_led_off(AO_LED_RED); + ao_led_off(AO_LED_PANIC); ao_beep(AO_BEEP_OFF); ao_panic_delay(10); } diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index 4f37e979..aac8fda5 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -113,6 +113,15 @@ ao_pyro_ready(struct ao_pyro *pyro) /* 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; } @@ -166,7 +175,7 @@ uint8_t ao_pyro_wakeup; static void ao_pyro(void) { - uint8_t p; + uint8_t p, any_waiting; struct ao_pyro *pyro; ao_config_get(); @@ -177,6 +186,7 @@ ao_pyro(void) ao_alarm(AO_MS_TO_TICKS(100)); ao_sleep(&ao_pyro_wakeup); ao_clear_alarm(); + any_waiting = 0; for (p = 0; p < AO_PYRO_NUM; p++) { pyro = &ao_config.pyro[p]; @@ -190,6 +200,7 @@ ao_pyro(void) if (!pyro->flags) continue; + any_waiting = 1; /* Check pyro state to see if it shoule fire */ if (!pyro->delay_done) { @@ -213,7 +224,10 @@ ao_pyro(void) ao_pyro_fire(p); } + if (!any_waiting) + break; } + ao_exit(); } __xdata struct ao_task ao_pyro_task; @@ -257,6 +271,9 @@ const struct { { "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") }, diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h index 5deb69d0..cde850ad 100644 --- a/src/core/ao_pyro.h +++ b/src/core/ao_pyro.h @@ -42,6 +42,9 @@ enum ao_pyro_flag { ao_pyro_after_motor = 0x00001000, ao_pyro_delay = 0x00002000, + + ao_pyro_state_less = 0x00004000, + ao_pyro_state_greater_or_equal = 0x00008000, }; struct ao_pyro { @@ -52,6 +55,7 @@ struct ao_pyro { 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; diff --git a/src/core/ao_radio_cmac.c b/src/core/ao_radio_cmac.c index fc0ca8b1..3ca3c313 100644 --- a/src/core/ao_radio_cmac.c +++ b/src/core/ao_radio_cmac.c @@ -85,18 +85,14 @@ radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant #if HAS_MONITOR ao_monitor_set(0); #endif - if (timeout) - ao_alarm(timeout); - - i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2); - ao_clear_alarm(); + 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 = (int8_t) (((int8_t) cmac_data[len + AO_CMAC_KEY_LEN]) >> 1) - 74; + 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; @@ -150,7 +146,7 @@ ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant int8_t ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant { - uint8_t i; + int8_t i; if (len > AO_CMAC_MAX_LEN) return AO_RADIO_CMAC_LEN_ERROR; ao_mutex_get(&ao_radio_cmac_mutex); diff --git a/src/core/ao_rssi.c b/src/core/ao_rssi.c index e3964d2d..244a84fe 100644 --- a/src/core/ao_rssi.c +++ b/src/core/ao_rssi.c @@ -17,9 +17,9 @@ #include "ao.h" -static __xdata volatile uint16_t ao_rssi_time; -static __pdata volatile uint16_t ao_rssi_delay; -static __pdata uint8_t ao_rssi_led; +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) diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index 985c0940..dec44f9f 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -37,6 +37,16 @@ __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_angle; +__pdata angle_t ao_sample_roll_angle; +#endif __data uint8_t ao_sample_data; @@ -53,6 +63,15 @@ __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 gyro_t ao_ground_pitch; +__pdata gyro_t ao_ground_yaw; +__pdata gyro_t ao_ground_roll; +#endif + static __pdata uint8_t ao_preflight; /* in preflight mode */ static __pdata uint16_t nsamples; @@ -60,6 +79,14 @@ __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; +#endif static void ao_sample_preflight_add(void) @@ -68,6 +95,14 @@ ao_sample_preflight_add(void) 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; } @@ -80,8 +115,23 @@ ao_sample_preflight_set(void) #endif ao_ground_pres = ao_sample_pres_sum >> 9; ao_ground_height = pres_to_altitude(ao_ground_pres); - nsamples = 0; 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 >> 9; + ao_ground_yaw = ao_sample_yaw_sum >> 9; + ao_ground_roll = ao_sample_roll_sum >> 9; + 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_angle = 0; +#endif + nsamples = 0; } static void @@ -122,6 +172,25 @@ ao_sample_preflight_update(void) 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) { @@ -147,6 +216,14 @@ ao_sample(void) 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(); @@ -154,6 +231,9 @@ ao_sample(void) if (ao_flight_state < ao_flight_boost) ao_sample_preflight_update(); ao_kalman(); +#if HAS_GYRO + /* do quaternion stuff here... */ +#endif } ao_sample_data = ao_data_ring_next(ao_sample_data); } @@ -171,6 +251,21 @@ ao_sample_init(void) 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_angle = 0; +#endif ao_sample_data = ao_data_head; ao_preflight = TRUE; } diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index 9336bdf9..a2dac979 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -111,6 +111,14 @@ 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 gyro_t ao_ground_pitch; +extern __pdata gyro_t ao_ground_yaw; +extern __pdata gyro_t ao_ground_roll; +#endif void ao_sample_init(void); diff --git a/src/core/ao_sample_profile.c b/src/core/ao_sample_profile.c new file mode 100644 index 00000000..1d9ed414 --- /dev/null +++ b/src/core/ao_sample_profile.c @@ -0,0 +1,173 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public 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_sample_profile.h> +#include <ao_task.h> + +#ifndef AO_SAMPLE_PROFILE_LOW_PC +#define AO_SAMPLE_PROFILE_LOW_PC 0x08000000 +#endif + +#ifndef AO_SAMPLE_PROFILE_HIGH_PC +#define AO_SAMPLE_PROFILE_HIGH_PC (AO_SAMPLE_PROFILE_LOW_PC + 44 * 1024) +#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 new file mode 100644 index 00000000..dbc29d3d --- /dev/null +++ b/src/core/ao_sample_profile.h @@ -0,0 +1,29 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public 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 <ao_sample_profile_timer.h> + +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 index 1a8e74de..66315d22 100644 --- a/src/core/ao_send_packet.c +++ b/src/core/ao_send_packet.c @@ -21,22 +21,6 @@ static __xdata uint8_t ao_send[AO_MAX_SEND]; -static uint8_t -getnibble(void) -{ - char c; - - c = getchar(); - if ('0' <= c && c <= '9') - return c - '0'; - if ('a' <= c && c <= 'f') - return c - ('a' - 10); - if ('A' <= c && c <= 'F') - return c - ('A' - 10); - ao_cmd_status = ao_cmd_lex_error; - return 0; -} - static void ao_send_packet(void) { @@ -53,8 +37,8 @@ ao_send_packet(void) return; } for (i = 0; i < count; i++) { - b = getnibble() << 4; - b |= getnibble(); + b = ao_getnibble() << 4; + b |= ao_getnibble(); if (ao_cmd_status != ao_cmd_success) return; ao_send[i] = b; diff --git a/src/core/ao_serial.h b/src/core/ao_serial.h index 53aa8a89..baf213c0 100644 --- a/src/core/ao_serial.h +++ b/src/core/ao_serial.h @@ -22,6 +22,7 @@ #define AO_SERIAL_SPEED_9600 1 #define AO_SERIAL_SPEED_19200 2 #define AO_SERIAL_SPEED_57600 3 +#define AO_SERIAL_SPEED_115200 4 #if HAS_SERIAL_0 extern volatile __xdata struct ao_fifo ao_serial0_rx_fifo; @@ -30,6 +31,9 @@ extern volatile __xdata struct ao_fifo ao_serial0_tx_fifo; char ao_serial0_getchar(void); +int +_ao_serial0_pollchar(void); + void ao_serial0_putchar(char c); @@ -47,8 +51,8 @@ extern volatile __xdata struct ao_fifo ao_serial1_tx_fifo; char ao_serial1_getchar(void); -char -ao_serial1_pollchar(void); +int +_ao_serial1_pollchar(void); void ao_serial1_putchar(char c); @@ -67,8 +71,8 @@ extern volatile __xdata struct ao_fifo ao_serial2_tx_fifo; char ao_serial2_getchar(void); -char -ao_serial2_pollchar(void); +int +_ao_serial2_pollchar(void); void ao_serial2_putchar(char c); @@ -80,6 +84,26 @@ void ao_serial2_set_speed(uint8_t speed); #endif +#if HAS_SERIAL_3 +extern volatile __xdata struct ao_fifo ao_serial3_rx_fifo; +extern volatile __xdata struct ao_fifo ao_serial3_tx_fifo; + +char +ao_serial3_getchar(void); + +int +_ao_serial3_pollchar(void); + +void +ao_serial3_putchar(char c); + +void +ao_serial3_drain(void); + +void +ao_serial3_set_speed(uint8_t speed); +#endif + void ao_serial_init(void); diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c index 09c2e319..3a550eaa 100644 --- a/src/core/ao_sqrt.c +++ b/src/core/ao_sqrt.c @@ -15,7 +15,9 @@ * 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 */ /** diff --git a/src/core/ao_stdio.c b/src/core/ao_stdio.c index 656b23c9..cd144d6b 100644 --- a/src/core/ao_stdio.c +++ b/src/core/ao_stdio.c @@ -66,8 +66,15 @@ #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) @@ -96,21 +103,28 @@ flush(void) __xdata uint8_t ao_stdin_ready; char -getchar(void) __reentrant __critical +getchar(void) __reentrant { - char c; - int8_t stdio = ao_cur_stdio; + int c; + int8_t stdio; + ao_arch_block_interrupts(); + stdio = ao_cur_stdio; for (;;) { - c = ao_stdios[stdio].pollchar(); + 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; } @@ -121,15 +135,21 @@ ao_echo(void) } int8_t -ao_add_stdio(char (*pollchar)(void), +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); - ao_stdios[ao_num_stdios].pollchar = pollchar; +#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_task.c b/src/core/ao_task.c index 65654731..0aad6508 100644 --- a/src/core/ao_task.c +++ b/src/core/ao_task.c @@ -16,14 +16,26 @@ */ #include <ao.h> +#include <ao_task.h> +#if HAS_SAMPLE_PROFILE +#include <ao_sample_profile.h> +#endif +#if HAS_STACK_GUARD +#include <ao_mpu.h> +#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; -__data uint8_t ao_cur_task_index; __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 @@ -42,6 +54,225 @@ static inline void ao_check_stack(void) { #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); + } + } +} +#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_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 { @@ -56,7 +287,6 @@ ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *nam if (t == ao_num_tasks) break; } - ao_tasks[ao_num_tasks++] = task; task->task_id = task_id; task->name = name; task->wchan = NULL; @@ -65,22 +295,50 @@ ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *nam * 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; @@ -88,6 +346,22 @@ ao_yield(void) ao_arch_naked_define /* 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 (;;) { @@ -106,13 +380,19 @@ ao_yield(void) ao_arch_naked_define (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) break; - /* Enter lower power mode when there isn't anything to do */ - if (ao_cur_task_index == ao_last_task_index) - ao_arch_cpu_idle(); + /* 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 - cli(); in_yield = 0; #endif ao_arch_restore_stack(); @@ -121,7 +401,15 @@ ao_yield(void) ao_arch_naked_define 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; @@ -134,28 +422,62 @@ ao_sleep(__xdata void *wchan) void ao_wakeup(__xdata void *wchan) { - uint8_t i; +#if HAS_TASK_QUEUE + struct ao_task *sleep, *next; + struct ao_list *sleep_queue; + uint32_t flags; - ao_check_stack(); + 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; @@ -171,17 +493,26 @@ ao_delay(uint16_t ticks) void ao_exit(void) { - ao_arch_critical( - uint8_t i; - ao_num_tasks--; - for (i = ao_cur_task_index; i < ao_num_tasks; i++) - ao_tasks[i] = ao_tasks[i+1]; - ao_cur_task_index = AO_NO_TASK_INDEX; - ao_yield(); - ); + 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) { @@ -194,12 +525,21 @@ ao_task_info(void) 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 new file mode 100644 index 00000000..1a4b5b6b --- /dev/null +++ b/src/core/ao_task.h @@ -0,0 +1,114 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public 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 <ao_list.h> +#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 +}; + +#define AO_NUM_TASKS 16 /* maximum number of tasks */ +#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); + +/* 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_telemetry.c b/src/core/ao_telemetry.c index 52ac9489..c3bbfec5 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -16,13 +16,20 @@ */ #include "ao.h" +#include "ao_log.h" #include "ao_product.h" static __pdata uint16_t ao_telemetry_interval; static __pdata uint8_t ao_rdf = 0; static __pdata uint16_t ao_rdf_time; -#if defined(MEGAMETRUM) +#if HAS_APRS +static __pdata uint16_t ao_aprs_time; + +#include <ao_aprs.h> +#endif + +#if defined(TELEMEGA) #define AO_SEND_MEGA 1 #endif @@ -223,6 +230,7 @@ ao_send_location(void) 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; @@ -288,30 +296,42 @@ ao_telemetry(void) while (ao_telemetry_interval == 0) ao_sleep(&telemetry); time = ao_rdf_time = ao_time(); +#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(); + ao_send_baro(); #endif +#if HAS_FLIGHT #ifdef AO_SEND_MEGA - ao_send_mega_sensor(); - ao_send_mega_data(); + ao_send_mega_sensor(); + ao_send_mega_data(); #else - ao_send_sensor(); + ao_send_sensor(); +#endif #endif #if HAS_COMPANION - if (ao_companion_running) - ao_send_companion(); + if (ao_companion_running) + ao_send_companion(); #endif - ao_send_configuration(); + ao_send_configuration(); #if HAS_GPS - ao_send_location(); - ao_send_satellite(); + ao_send_location(); + ao_send_satellite(); #endif + } #ifndef AO_SEND_ALL_BARO if (ao_rdf && +#if HAS_APRS + !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && +#endif (int16_t) (ao_time() - ao_rdf_time) >= 0) { #if HAS_IGNITE_REPORT @@ -325,6 +345,14 @@ ao_telemetry(void) #endif ao_radio_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 #endif time += ao_telemetry_interval; delay = time - ao_time(); @@ -389,8 +417,9 @@ ao_rdf_set(uint8_t rdf) ao_rdf = rdf; if (rdf == 0) ao_radio_rdf_abort(); - else + else { ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; + } } __xdata struct ao_task ao_telemetry_task; diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h index 32a1668c..f2d201de 100644 --- a/src/core/ao_telemetry.h +++ b/src/core/ao_telemetry.h @@ -126,13 +126,15 @@ struct ao_telemetry_satellite_info { 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[12]; /* 6 */ + struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT]; /* 6 */ uint8_t unused[2]; /* 30 */ /* 32 */ }; diff --git a/src/core/ao_usb.h b/src/core/ao_usb.h index e051db93..4476ee6b 100644 --- a/src/core/ao_usb.h +++ b/src/core/ao_usb.h @@ -33,7 +33,7 @@ ao_usb_getchar(void); /* Poll for a charcter on the USB input queue. * returns AO_READ_AGAIN if none are available */ -char +int ao_usb_pollchar(void); /* Flush the USB output queue */ |