diff options
Diffstat (limited to 'src/kernel')
| -rw-r--r-- | src/kernel/ao_flash_readout.c | 50 | ||||
| -rw-r--r-- | src/kernel/ao_flash_readout.h | 20 | ||||
| -rw-r--r-- | src/kernel/ao_flight.c | 11 | ||||
| -rw-r--r-- | src/kernel/ao_flight.h | 1 | ||||
| -rw-r--r-- | src/kernel/ao_kalman.c | 10 | ||||
| -rw-r--r-- | src/kernel/ao_product.c | 14 | ||||
| -rw-r--r-- | src/kernel/ao_pyro.c | 36 | ||||
| -rw-r--r-- | src/kernel/ao_sample.c | 83 | ||||
| -rw-r--r-- | src/kernel/ao_sample.h | 3 | ||||
| -rw-r--r-- | src/kernel/ao_usb.h | 21 |
10 files changed, 203 insertions, 46 deletions
diff --git a/src/kernel/ao_flash_readout.c b/src/kernel/ao_flash_readout.c new file mode 100644 index 00000000..46b5ba7a --- /dev/null +++ b/src/kernel/ao_flash_readout.c @@ -0,0 +1,50 @@ +/* + * Copyright © 2018 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, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +#include <ao.h> +#include <ao_usb.h> +#include <ao_flash_readout.h> + +#ifndef AO_FLASH_READOUT_BASE +#define AO_FLASH_READOUT_BASE AO_BOOT_LOADER_BASE +#define AO_FLASH_READOUT_BOUND AO_BOOT_APPLICATION_BOUND +#endif + +static void +ao_flash_readout(void) +{ + uint8_t *base = (uint8_t *) AO_FLASH_READOUT_BASE; + uint8_t *bound = (uint8_t *) AO_FLASH_READOUT_BOUND; + uint8_t *p = base; + + for (;;) { + ao_arch_block_interrupts(); + while (!ao_usb_running) { + p = base; + ao_sleep(&ao_usb_running); + } + ao_arch_release_interrupts(); + ao_flash_readout_putchar(*p++); + if (p == bound) + p = base; + } +} + +static struct ao_task ao_flash_readout_task; + +void +ao_flash_readout_init(void) +{ + ao_add_task(&ao_flash_readout_task, ao_flash_readout, "flash_readout"); +} diff --git a/src/kernel/ao_flash_readout.h b/src/kernel/ao_flash_readout.h new file mode 100644 index 00000000..5eee53cc --- /dev/null +++ b/src/kernel/ao_flash_readout.h @@ -0,0 +1,20 @@ +/* + * Copyright © 2018 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, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +#ifndef _AO_FLASH_READOUT_H_ +#define _AO_FLASH_READOUT_H_ + +void ao_flash_readout_init(void); + +#endif /* _AO_FLASH_READOUT_H_ */ diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index cb02c454..c2700d20 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -48,7 +48,8 @@ /* 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_boost_tick; /* time of most recent boost detect */ +__pdata uint16_t ao_launch_tick; /* time of first boost detect */ __pdata uint16_t ao_motor_number; /* number of motors burned so far */ #if HAS_SENSOR_ERRORS @@ -199,7 +200,7 @@ ao_flight(void) ) { ao_flight_state = ao_flight_boost; - ao_boost_tick = ao_sample_tick; + ao_launch_tick = ao_boost_tick = ao_sample_tick; /* start logging data */ ao_log_start(); @@ -233,7 +234,7 @@ ao_flight(void) * 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)) || + if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)) || (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) { #if HAS_ACCEL @@ -269,7 +270,7 @@ ao_flight(void) * number of seconds. */ if (ao_config.apogee_lockout) { - if ((int16_t) (ao_sample_tick - ao_boost_tick) < + if ((int16_t) (ao_sample_tick - ao_launch_tick) < AO_SEC_TO_TICKS(ao_config.apogee_lockout)) break; } @@ -310,7 +311,7 @@ ao_flight(void) #if HAS_ACCEL else { check_re_boost: - ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6); + ao_coast_avg_accel = ao_coast_avg_accel + ((ao_accel - ao_coast_avg_accel) >> 5); if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) { ao_boost_tick = ao_sample_tick; ao_flight_state = ao_flight_boost; diff --git a/src/kernel/ao_flight.h b/src/kernel/ao_flight.h index 6894fe59..005c7e84 100644 --- a/src/kernel/ao_flight.h +++ b/src/kernel/ao_flight.h @@ -40,6 +40,7 @@ enum ao_flight_state { extern __pdata enum ao_flight_state ao_flight_state; extern __pdata uint16_t ao_boost_tick; +extern __pdata uint16_t ao_launch_tick; extern __pdata uint16_t ao_motor_number; #if HAS_IMU || HAS_MMA655X diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index ac41085d..e4cc6d4b 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -45,7 +45,11 @@ static __pdata ao_k_t ao_avg_height_scaled; __xdata ao_v_t ao_avg_height; __pdata ao_v_t ao_error_h; -#if !HAS_ACCEL +#if !HAS_ACCEL || AO_FLIGHT_TEST +#define AO_ERROR_H_SQ_AVG 1 +#endif + +#if AO_ERROR_H_SQ_AVG __pdata ao_v_t ao_error_h_sq_avg; #endif @@ -85,7 +89,7 @@ ao_kalman_predict(void) static void ao_kalman_err_height(void) { -#if !HAS_ACCEL +#if AO_ERROR_H_SQ_AVG ao_v_t e; #endif ao_v_t height_distrust; @@ -95,7 +99,7 @@ ao_kalman_err_height(void) ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16); -#if !HAS_ACCEL +#if AO_ERROR_H_SQ_AVG e = ao_error_h; if (e < 0) e = -e; diff --git a/src/kernel/ao_product.c b/src/kernel/ao_product.c index c4df9f26..4c2d83ef 100644 --- a/src/kernel/ao_product.c +++ b/src/kernel/ao_product.c @@ -55,7 +55,7 @@ const char ao_product[] = AO_iProduct_STRING; #define HEADER_LEN 9 #define CONTROL_CLASS_LEN 35 -#define DATA_LEN (9 + 7 * AO_USB_HAS_OUT + 7 * AO_USB_HAS_IN + 7 * AO_USB_HAS_IN2) +#define DATA_LEN (9 + 7 * AO_USB_HAS_OUT + 7 * AO_USB_HAS_IN + 7 * AO_USB_HAS_IN2 + 7 * AO_USB_HAS_IN3) #define TOTAL_LENGTH (HEADER_LEN + AO_USB_HAS_INT * CONTROL_CLASS_LEN + DATA_LEN) #define NUM_INTERFACES (AO_USB_HAS_INT + 1) @@ -141,7 +141,7 @@ AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = AO_USB_DESC_INTERFACE, AO_USB_HAS_INT, /* bInterfaceNumber */ 0x00, /* bAlternateSetting */ - AO_USB_HAS_OUT + AO_USB_HAS_IN + AO_USB_HAS_IN2, /* bNumEndPoints */ + AO_USB_HAS_OUT + AO_USB_HAS_IN + AO_USB_HAS_IN2 + AO_USB_HAS_IN3, /* bNumEndPoints */ AO_USB_INTERFACE_CLASS_DATA, /* bInterfaceClass = data */ 0x00, /* bInterfaceSubClass */ 0x00, /* bInterfaceProtocol */ @@ -177,6 +177,16 @@ AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = 0x00, /* bInterval */ #endif +#if AO_USB_HAS_IN3 + /* Data EP in 3 */ + 0x07, + AO_USB_DESC_ENDPOINT, + AO_USB_IN3_EP|0x80, /* bEndpointAddress */ + 0x02, /* bmAttributes = bulk */ + LE_WORD(AO_USB_IN_SIZE),/* wMaxPacketSize */ + 0x00, /* bInterval */ +#endif + /* String descriptors */ 0x04, AO_USB_DESC_STRING, diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index e5c30eec..527112ac 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -81,6 +81,19 @@ int pyro_dbg; #define DBG(...) #endif +static angle_t +ao_sample_max_orient(void) +{ + uint8_t i; + angle_t max = ao_sample_orients[0]; + + for (i = 1; i < AO_NUM_ORIENT; i++) { + angle_t a = ao_sample_orients[i]; + if (a > max) + max = a; + } + return max; +} /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -90,6 +103,9 @@ static uint8_t ao_pyro_ready(struct ao_pyro *pyro) { enum ao_pyro_flag flag, flags; +#if HAS_GYRO + angle_t max_orient; +#endif flags = pyro->flags; while (flags != ao_pyro_none) { @@ -130,26 +146,28 @@ ao_pyro_ready(struct ao_pyro *pyro) #if HAS_GYRO case ao_pyro_orient_less: - if (ao_sample_orient <= pyro->orient_less) + max_orient = ao_sample_max_orient(); + if (max_orient <= pyro->orient_less) continue; - DBG("orient %d > %d\n", ao_sample_orient, pyro->orient_less); + DBG("orient %d > %d\n", max_orient, pyro->orient_less); break; case ao_pyro_orient_greater: - if (ao_sample_orient >= pyro->orient_greater) + max_orient = ao_sample_max_orient(); + if (max_orient >= pyro->orient_greater) continue; - DBG("orient %d < %d\n", ao_sample_orient, pyro->orient_greater); + DBG("orient %d < %d\n", max_orient, pyro->orient_greater); break; #endif case ao_pyro_time_less: - if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) + if ((int16_t) (ao_time() - ao_launch_tick) <= pyro->time_less) continue; - DBG("time %d > %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_less); + DBG("time %d > %d\n", (int16_t)(ao_time() - ao_launch_tick), pyro->time_less); break; case ao_pyro_time_greater: - if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) + if ((int16_t) (ao_time() - ao_launch_tick) >= pyro->time_greater) continue; - DBG("time %d < %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_greater); + DBG("time %d < %d\n", (int16_t)(ao_time() - ao_launch_tick), pyro->time_greater); break; case ao_pyro_ascending: @@ -164,7 +182,7 @@ ao_pyro_ready(struct ao_pyro *pyro) break; case ao_pyro_after_motor: - if (ao_motor_number == pyro->motor) + if (ao_motor_number >= pyro->motor) continue; DBG("motor %d != %d\n", ao_motor_number, pyro->motor); break; diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 61519478..f8012e34 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -50,6 +50,8 @@ __pdata gyro_t ao_sample_roll; __pdata gyro_t ao_sample_pitch; __pdata gyro_t ao_sample_yaw; __pdata angle_t ao_sample_orient; +__pdata angle_t ao_sample_orients[AO_NUM_ORIENT]; +__pdata uint8_t ao_sample_orient_pos; #endif __data uint8_t ao_sample_data; @@ -115,6 +117,53 @@ ao_sample_preflight_add(void) ++nsamples; } +#if HAS_GYRO +static void +ao_sample_set_all_orients(void) +{ + int i; + for (i = 0; i < AO_NUM_ORIENT; i++) + ao_sample_orients[i] = ao_sample_orient; + ao_sample_orient_pos = 0; +} + +static void +ao_sample_set_one_orient(void) +{ + ao_sample_orients[ao_sample_orient_pos] = ao_sample_orient; + ao_sample_orient_pos = (ao_sample_orient_pos + 1) % AO_NUM_ORIENT; +} + +static void +ao_sample_compute_orient(void) +{ + /* 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); +} +#endif /* HAS_GYRO */ + static void ao_sample_preflight_set(void) { @@ -138,7 +187,7 @@ ao_sample_preflight_set(void) ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_sample_orient = 0; + ao_sample_set_all_orients(); struct ao_quaternion orient; @@ -168,6 +217,9 @@ ao_sample_preflight_set(void) if (ao_orient_test) printf("\n\treset\n"); #endif + + ao_sample_compute_orient(); + ao_sample_set_all_orients(); #endif nsamples = 0; } @@ -195,31 +247,6 @@ ao_sample_rotate(void) /* 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", @@ -229,7 +256,8 @@ ao_sample_rotate(void) ao_sample_orient); } #endif - + ao_sample_compute_orient(); + ao_sample_set_one_orient(); } #endif @@ -367,6 +395,7 @@ ao_sample_init(void) ao_sample_yaw = 0; ao_sample_roll = 0; ao_sample_orient = 0; + ao_sample_set_all_orients(); #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index fbef031d..5ae389be 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -146,7 +146,10 @@ 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; +#define AO_NUM_ORIENT 64 extern __pdata angle_t ao_sample_orient; +extern __pdata angle_t ao_sample_orients[AO_NUM_ORIENT]; +extern __pdata uint8_t ao_sample_orient_pos; #endif void ao_sample_init(void); diff --git a/src/kernel/ao_usb.h b/src/kernel/ao_usb.h index 936d939b..40516de1 100644 --- a/src/kernel/ao_usb.h +++ b/src/kernel/ao_usb.h @@ -41,7 +41,23 @@ ao_usb_pollchar(void); void ao_usb_flush(void); +#if AO_USB_HAS_IN2 +void +ao_usb_flush2(void); + +void +ao_usb_putchar2(char c); +#endif + +#if AO_USB_HAS_IN3 +void +ao_usb_flush3(void); + +void +ao_usb_putchar3(char c); +#endif /* Enable the USB controller */ + void ao_usb_enable(void); @@ -107,6 +123,7 @@ extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; #define AO_USB_OUT_EP 4 #define AO_USB_IN_EP 5 #define AO_USB_IN2_EP 6 +#define AO_USB_IN3_EP 7 #endif #ifndef AO_USB_HAS_OUT @@ -125,6 +142,10 @@ extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; #define AO_USB_HAS_IN2 0 #endif +#ifndef AO_USB_HAS_IN3 +#define AO_USB_HAS_IN3 0 +#endif + /* * USB bulk packets can only come in 8, 16, 32 and 64 * byte sizes, so we'll use 64 for everything |