From 5e9193f6375be27e5f7a0321fd34b6acfe81247f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 09:12:29 -0700 Subject: altos: Add 'g' command to ublox GPS code. Take the gps_dump function from ao_gps_skytraq.c and move it to a new file so it can be shared with the u-blox driver. That affects every skytraq and u-blox user as they need to include the new file. Signed-off-by: Keith Packard --- src/core/ao.h | 3 +++ src/core/ao_gps_show.c | 39 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 42 insertions(+) create mode 100644 src/core/ao_gps_show.c (limited to 'src/core') diff --git a/src/core/ao.h b/src/core/ao.h index 0ad3e4aa..71bfb6a1 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -379,6 +379,9 @@ 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); diff --git a/src/core/ao_gps_show.c b/src/core/ao_gps_show.c new file mode 100644 index 00000000..3a05e35a --- /dev/null +++ b/src/core/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); +} -- cgit v1.2.3 From 1931e028bebc3cd8df9392e30eb0e888d0799768 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 22:29:06 -0700 Subject: altos: Move MS5607 info from 'v' to 'c s' Makes more sense there. Signed-off-by: Keith Packard --- src/core/ao_cmd.c | 3 --- src/core/ao_config.c | 3 +++ 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c index 188b8bb4..5113548b 100644 --- a/src/core/ao_cmd.c +++ b/src/core/ao_cmd.c @@ -290,9 +290,6 @@ version(void) , ao_log_format #endif ); -#if HAS_MS5607 - ao_ms5607_info(); -#endif printf("software-version %s\n", ao_version); } #endif diff --git a/src/core/ao_config.c b/src/core/ao_config.c index 73608a55..1a500c79 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -693,6 +693,9 @@ ao_config_show(void) __reentrant 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 -- cgit v1.2.3 From 8a19805a6b079450b5afd5fa2334cede8495ae4a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 17 May 2013 03:21:08 -0700 Subject: altos/cc1111: Hack on USB driver to make Windows happy The Windows modem driver is quite chatty at startup time, getting and setting the comm parameters each time the device is opened. Sometimes, when setting the parameters, the cc1111 would STALL EP0. Most of the time, Windows would happily pass this as an error back to AltosUI which would then re-try the open (and succeed, most of the time). Sometimes, Windows would stall for 30 seconds before passing the error back. This made the whole UI freeze, and I suspect most people assumed our app had died. A bit of analysis with the beagle USB sniffer and I discovered the STALL settings, but there wasn't any correlation between the data on the wire and when the STALL would be generated. So, I found a couple of other cc1111 USB stacks on the net and just looked to see how our driver differed. There wasn't anything clearly related, but there were a list of small differences: 1) Other drivers didn't bother waiting for the hardware to ack the USBADDR setting; doing it this way means we can set the address *before* acking the setup packet. It'll get set eventually, at which point the device will start responding to packets again. Easy to fix, and saves a bit of code space too. 2) The other drivers set the STALL bit for setup packets which aren't understood. This shouldn't have any effect on 'good' systems as those shouldn't ever be generating bogus setup packets anyways. The driver already handled the STALL state in the interrupt handler, the only requirement was to figure out when to explicitly set the STALL bit. That required moving the state updating code from the start of the ep0 setup handling to the end, after the setup packet had been examined and data queued in or out as appropriate. 3) Our driver explicitly queued an IN packet for any setup request that wasn't waiting for an OUT pack. This appears to tie in with the USBADDR change above as before I made that change, this change caused the driver to fail to respond to most setup packets. This was simple once the above change was made, just move the generation of the IN packet inside the code that switched to the IN state. Signed-off-by: Keith Packard --- src/cc1111/ao_usb.c | 68 ++++++++++++++++++++++++++++++----------------------- src/core/ao_usb.h | 1 + 2 files changed, 39 insertions(+), 30 deletions(-) (limited to 'src/core') diff --git a/src/cc1111/ao_usb.c b/src/cc1111/ao_usb.c index 8bd2efdf..a655d1be 100644 --- a/src/cc1111/ao_usb.c +++ b/src/cc1111/ao_usb.c @@ -152,19 +152,17 @@ ao_usb_ep0_fill(void) *ao_usb_ep0_out_data++ = USBFIFO[0]; } -void +static void ao_usb_ep0_queue_byte(uint8_t a) { ao_usb_ep0_in_buf[ao_usb_ep0_in_len++] = a; } -void +static void ao_usb_set_address(uint8_t address) { ao_usb_running = 1; - USBADDR = address | 0x80; - while (USBADDR & 0x80) - ; + USBADDR = address; } static void @@ -191,24 +189,6 @@ ao_usb_ep0_setup(void) if (ao_usb_ep0_out_len != 0) return; - /* Figure out how to ACK the setup packet */ - if (ao_usb_setup.dir_type_recip & AO_USB_DIR_IN) { - if (ao_usb_setup.length) - ao_usb_ep0_state = AO_USB_EP0_DATA_IN; - else - ao_usb_ep0_state = AO_USB_EP0_IDLE; - } else { - if (ao_usb_setup.length) - ao_usb_ep0_state = AO_USB_EP0_DATA_OUT; - else - ao_usb_ep0_state = AO_USB_EP0_IDLE; - } - USBINDEX = 0; - if (ao_usb_ep0_state == AO_USB_EP0_IDLE) - USBCS0 = USBCS0_CLR_OUTPKT_RDY | USBCS0_DATA_END; - else - USBCS0 = USBCS0_CLR_OUTPKT_RDY; - ao_usb_ep0_in_data = ao_usb_ep0_in_buf; ao_usb_ep0_in_len = 0; switch(ao_usb_setup.dir_type_recip & AO_USB_SETUP_TYPE_MASK) { @@ -274,10 +254,39 @@ ao_usb_ep0_setup(void) } break; } - if (ao_usb_ep0_state != AO_USB_EP0_DATA_OUT) { + + /* Figure out how to ACK the setup packet and the + * next state + */ + USBINDEX = 0; + if (ao_usb_ep0_in_len) { + + /* Sending data back to the host + */ + ao_usb_ep0_state = AO_USB_EP0_DATA_IN; + USBCS0 = USBCS0_CLR_OUTPKT_RDY; if (ao_usb_setup.length < ao_usb_ep0_in_len) ao_usb_ep0_in_len = ao_usb_setup.length; ao_usb_ep0_flush(); + } else if (ao_usb_ep0_out_len) { + + /* Receiving data from the host + */ + ao_usb_ep0_state = AO_USB_EP0_DATA_OUT; + USBCS0 = USBCS0_CLR_OUTPKT_RDY; + } else if (ao_usb_setup.length) { + + /* Uh-oh, the host expected to send or receive data + * and we don't know what to do. + */ + ao_usb_ep0_state = AO_USB_EP0_STALL; + USBCS0 = USBCS0_CLR_OUTPKT_RDY | USBCS0_SEND_STALL; + } else { + + /* Simple setup packet with no data + */ + ao_usb_ep0_state = AO_USB_EP0_IDLE; + USBCS0 = USBCS0_CLR_OUTPKT_RDY | USBCS0_DATA_END; } } @@ -299,12 +308,12 @@ ao_usb_ep0(void) USBINDEX = 0; cs0 = USBCS0; if (cs0 & USBCS0_SETUP_END) { - ao_usb_ep0_state = AO_USB_EP0_IDLE; USBCS0 = USBCS0_CLR_SETUP_END; + ao_usb_ep0_state = AO_USB_EP0_IDLE; } if (cs0 & USBCS0_SENT_STALL) { + USBCS0 = 0; ao_usb_ep0_state = AO_USB_EP0_IDLE; - USBCS0 &= ~USBCS0_SENT_STALL; } if (ao_usb_ep0_state == AO_USB_EP0_DATA_IN && (cs0 & USBCS0_INPKT_RDY) == 0) @@ -318,12 +327,11 @@ ao_usb_ep0(void) break; case AO_USB_EP0_DATA_OUT: ao_usb_ep0_fill(); - if (ao_usb_ep0_out_len == 0) - ao_usb_ep0_state = AO_USB_EP0_IDLE; USBINDEX = 0; - if (ao_usb_ep0_state == AO_USB_EP0_IDLE) + if (ao_usb_ep0_out_len == 0) { + ao_usb_ep0_state = AO_USB_EP0_IDLE; USBCS0 = USBCS0_CLR_OUTPKT_RDY | USBCS0_DATA_END; - else + } else USBCS0 = USBCS0_CLR_OUTPKT_RDY; break; } diff --git a/src/core/ao_usb.h b/src/core/ao_usb.h index 4476ee6b..6bc77608 100644 --- a/src/core/ao_usb.h +++ b/src/core/ao_usb.h @@ -114,6 +114,7 @@ extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; #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)) -- cgit v1.2.3 From 185e6d15bcda229949a984910d7394203d301db9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 16 May 2013 18:58:24 -0700 Subject: altos: Allow target-specific USB endpoint specifications The LPC has only a small number of endpoints, and those are not configurable. Let the LPC USB driver pick the IN and OUT endpoints by itself. Signed-off-by: Keith Packard --- src/core/ao_usb.h | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/core') diff --git a/src/core/ao_usb.h b/src/core/ao_usb.h index 6bc77608..35e64e65 100644 --- a/src/core/ao_usb.h +++ b/src/core/ao_usb.h @@ -102,8 +102,11 @@ extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; #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 -- cgit v1.2.3 From ca4f3161258356c06fe1270f7ccdf0d6939e2d34 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 18 May 2013 02:52:49 -0700 Subject: altos: Move ao_data.c from stm to core This should be used on every processor Signed-off-by: Keith Packard --- src/core/ao_data.c | 34 ++++++++++++++++++++++++++++++++++ src/stm/ao_data.c | 34 ---------------------------------- 2 files changed, 34 insertions(+), 34 deletions(-) create mode 100644 src/core/ao_data.c delete mode 100644 src/stm/ao_data.c (limited to 'src/core') diff --git a/src/core/ao_data.c b/src/core/ao_data.c new file mode 100644 index 00000000..38d2f7ff --- /dev/null +++ b/src/core/ao_data.c @@ -0,0 +1,34 @@ +/* + * 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; + +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)); +} diff --git a/src/stm/ao_data.c b/src/stm/ao_data.c deleted file mode 100644 index 38d2f7ff..00000000 --- a/src/stm/ao_data.c +++ /dev/null @@ -1,34 +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; - -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)); -} -- cgit v1.2.3 From e4385d29fc1b233b3ad56d4af68a175e760c1751 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 18 May 2013 02:53:32 -0700 Subject: altos: Allow architecture to define the type of port registers LPC11U14 has 32-bit ports, STM32 has 16 bit ports. Signed-off-by: Keith Packard --- src/core/ao.h | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src/core') diff --git a/src/core/ao.h b/src/core/ao.h index 71bfb6a1..7f344736 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -43,6 +43,12 @@ #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 @@ -68,6 +74,7 @@ #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_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 */ -- cgit v1.2.3 From 52063c2679752033135fff928c7686e368d2a825 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 18 May 2013 02:54:30 -0700 Subject: altos: ao_data_get is in ao_data.c now, not ao_adc.c Signed-off-by: Keith Packard --- src/core/ao_adc.h | 4 ---- src/core/ao_data.h | 4 ++++ 2 files changed, 4 insertions(+), 4 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_adc.h b/src/core/ao_adc.h index 0dd87080..373db1c4 100644 --- a/src/core/ao_adc.h +++ b/src/core/ao_adc.h @@ -28,10 +28,6 @@ ao_adc_poll(void); void ao_adc_sleep(void); -/* Get a copy of the last complete sample set */ -void -ao_data_get(__xdata struct ao_data *packet); - /* Initialize the A/D converter */ void ao_adc_init(void); diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 7e2f85d8..b0f086f8 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -82,6 +82,10 @@ struct ao_data { #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; -- cgit v1.2.3 From 82afe3a3b737c43dbeaad41ea5af1841357297a6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 18 May 2013 02:54:55 -0700 Subject: altos: Check for packet mode before trying to disable it in flight code This is only relevant for telemini Signed-off-by: Keith Packard --- src/core/ao_flight.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 9d9d4c6e..782e2274 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -124,7 +124,7 @@ ao_flight(void) ao_usb_disable(); #endif -#if !HAS_ACCEL +#if !HAS_ACCEL && PACKET_HAS_SLAVE /* Disable packet mode in pad state on TeleMini */ ao_packet_slave_stop(); #endif -- cgit v1.2.3 From 28890aa5893898cd0bb0ac033e491eb307a84ca5 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 18 May 2013 03:02:01 -0700 Subject: altos: Use ao_data_pres macro in ao_log_tiny Now it works on easymini too Signed-off-by: Keith Packard --- src/core/ao_log_tiny.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_log_tiny.c b/src/core/ao_log_tiny.c index 492658ea..67767dc9 100644 --- a/src/core/ao_log_tiny.c +++ b/src/core/ao_log_tiny.c @@ -105,7 +105,7 @@ ao_log(void) */ ao_sleep(DATA_TO_XDATA(&ao_sample_data)); while (ao_log_data != ao_sample_data) { - sum += ao_data_ring[ao_log_data].adc.pres; + sum += ao_data_pres(&ao_data_ring[ao_log_data]); count++; ao_log_data = ao_data_ring_next(ao_log_data); } -- cgit v1.2.3 From 57b4d82dee10b142b820aa306028a288a85214f6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 19 May 2013 23:07:54 -0700 Subject: Add Mini logging format. Use in EasyMini This is a 16-byte record that includes all of the sensor data in each sensor record, along with records for flight state changes. Signed-off-by: Keith Packard --- altoslib/AltosEepromMini.java | 193 ++++++++++++++++++++++ altoslib/AltosEepromMiniIterable.java | 297 ++++++++++++++++++++++++++++++++++ altoslib/AltosLib.java | 1 + altoslib/AltosOrderedMiniRecord.java | 52 ++++++ altoslib/AltosRecordMini.java | 129 +++++++++++++++ altoslib/Makefile.am | 4 + altosui/AltosDataChooser.java | 7 +- altosui/AltosEepromDownload.java | 50 ++++++ altosui/AltosLanded.java | 3 + altosui/AltosUI.java | 4 + src/core/ao_log.h | 38 +++++ src/core/ao_log_mini.c | 163 +++++++++++++++++++ src/easymini-v0.1/Makefile | 2 +- src/easymini-v0.1/ao_pins.h | 9 +- 14 files changed, 947 insertions(+), 5 deletions(-) create mode 100644 altoslib/AltosEepromMini.java create mode 100644 altoslib/AltosEepromMiniIterable.java create mode 100644 altoslib/AltosOrderedMiniRecord.java create mode 100644 altoslib/AltosRecordMini.java create mode 100644 src/core/ao_log_mini.c (limited to 'src/core') diff --git a/altoslib/AltosEepromMini.java b/altoslib/AltosEepromMini.java new file mode 100644 index 00000000..215cd3d9 --- /dev/null +++ b/altoslib/AltosEepromMini.java @@ -0,0 +1,193 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +import java.text.*; + +public class AltosEepromMini { + public int cmd; + public int tick; + public boolean valid; + public String data; + public int config_a, config_b; + + public int data8[]; + + public static final int record_length = 16; + static final int header_length = 4; + static final int data_length = record_length - header_length; + + public int data8(int i) { + return data8[i]; + } + + public int data16(int i) { + return ((data8[i] | (data8[i+1] << 8)) << 16) >> 16; + } + + public int data24(int i) { + return data8[i] | (data8[i+1] << 8) | (data8[i+2] << 16); + } + + public int data32(int i) { + return data8[i] | (data8[i+1] << 8) | (data8[i+2] << 16) | (data8[i+3] << 24); + } + + /* AO_LOG_FLIGHT elements */ + public int flight() { return data16(0); } + public int ground_pres() { return data32(4); } + + /* AO_LOG_STATE elements */ + public int state() { return data16(0); } + public int reason() { return data16(2); } + + /* AO_LOG_SENSOR elements */ + public int pres() { return data24(0); } + public int temp() { return data24(3); } + public int sense_a() { return data16(6); } + public int sense_m() { return data16(8); } + public int v_batt() { return data16(10); } + + public AltosEepromMini (AltosEepromChunk chunk, int start) throws ParseException { + cmd = chunk.data(start); + + valid = !chunk.erased(start, record_length); + if (valid) { + if (AltosConvert.checksum(chunk.data, start, record_length) != 0) + throw new ParseException(String.format("invalid checksum at 0x%x", + chunk.address + start), 0); + } else { + cmd = AltosLib.AO_LOG_INVALID; + } + + tick = chunk.data16(start+2); + + data8 = new int[data_length]; + for (int i = 0; i < data_length; i++) + data8[i] = chunk.data(start + header_length + i); + } + + public AltosEepromMini (String line) { + valid = false; + tick = 0; + + if (line == null) { + cmd = AltosLib.AO_LOG_INVALID; + line = ""; + } else { + try { + String[] tokens = line.split("\\s+"); + + if (tokens[0].length() == 1) { + if (tokens.length != 2 + data_length) { + cmd = AltosLib.AO_LOG_INVALID; + data = line; + } else { + cmd = tokens[0].codePointAt(0); + tick = Integer.parseInt(tokens[1],16); + valid = true; + data8 = new int[data_length]; + for (int i = 0; i < data_length; i++) + data8[i] = Integer.parseInt(tokens[2 + i],16); + } + } else if (tokens[0].equals("Config") && tokens[1].equals("version:")) { + cmd = AltosLib.AO_LOG_CONFIG_VERSION; + data = tokens[2]; + } else if (tokens[0].equals("Main") && tokens[1].equals("deploy:")) { + cmd = AltosLib.AO_LOG_MAIN_DEPLOY; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Apogee") && tokens[1].equals("delay:")) { + cmd = AltosLib.AO_LOG_APOGEE_DELAY; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Radio") && tokens[1].equals("channel:")) { + cmd = AltosLib.AO_LOG_RADIO_CHANNEL; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Callsign:")) { + cmd = AltosLib.AO_LOG_CALLSIGN; + data = tokens[1].replaceAll("\"",""); + } else if (tokens[0].equals("Accel") && tokens[1].equals("cal")) { + cmd = AltosLib.AO_LOG_ACCEL_CAL; + config_a = Integer.parseInt(tokens[3]); + config_b = Integer.parseInt(tokens[5]); + } else if (tokens[0].equals("Radio") && tokens[1].equals("cal:")) { + cmd = AltosLib.AO_LOG_RADIO_CAL; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[0].equals("Max") && tokens[1].equals("flight") && tokens[2].equals("log:")) { + cmd = AltosLib.AO_LOG_MAX_FLIGHT_LOG; + config_a = Integer.parseInt(tokens[3]); + } else if (tokens[0].equals("manufacturer")) { + cmd = AltosLib.AO_LOG_MANUFACTURER; + data = tokens[1]; + } else if (tokens[0].equals("product")) { + cmd = AltosLib.AO_LOG_PRODUCT; + data = tokens[1]; + } else if (tokens[0].equals("serial-number")) { + cmd = AltosLib.AO_LOG_SERIAL_NUMBER; + config_a = Integer.parseInt(tokens[1]); + } else if (tokens[0].equals("log-format")) { + cmd = AltosLib.AO_LOG_LOG_FORMAT; + config_a = Integer.parseInt(tokens[1]); + } else if (tokens[0].equals("software-version")) { + cmd = AltosLib.AO_LOG_SOFTWARE_VERSION; + data = tokens[1]; + } else if (tokens[0].equals("ms5607")) { + if (tokens[1].equals("reserved:")) { + cmd = AltosLib.AO_LOG_BARO_RESERVED; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("sens:")) { + cmd = AltosLib.AO_LOG_BARO_SENS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("off:")) { + cmd = AltosLib.AO_LOG_BARO_OFF; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tcs:")) { + cmd = AltosLib.AO_LOG_BARO_TCS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tco:")) { + cmd = AltosLib.AO_LOG_BARO_TCO; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tref:")) { + cmd = AltosLib.AO_LOG_BARO_TREF; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("tempsens:")) { + cmd = AltosLib.AO_LOG_BARO_TEMPSENS; + config_a = Integer.parseInt(tokens[2]); + } else if (tokens[1].equals("crc:")) { + cmd = AltosLib.AO_LOG_BARO_CRC; + config_a = Integer.parseInt(tokens[2]); + } else { + cmd = AltosLib.AO_LOG_INVALID; + data = line; + } + } else { + cmd = AltosLib.AO_LOG_INVALID; + data = line; + } + } catch (NumberFormatException ne) { + cmd = AltosLib.AO_LOG_INVALID; + data = line; + } + } + } + + public AltosEepromMini(int in_cmd, int in_tick) { + cmd = in_cmd; + tick = in_tick; + valid = true; + } +} diff --git a/altoslib/AltosEepromMiniIterable.java b/altoslib/AltosEepromMiniIterable.java new file mode 100644 index 00000000..1f221187 --- /dev/null +++ b/altoslib/AltosEepromMiniIterable.java @@ -0,0 +1,297 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromMiniIterable extends AltosRecordIterable { + + static final int seen_flight = 1; + static final int seen_sensor = 2; + + static final int seen_basic = seen_flight|seen_sensor; + + boolean has_accel; + boolean has_gps; + boolean has_ignite; + + AltosEepromMini flight_record; + + TreeSet records; + + AltosMs5607 baro; + + LinkedList list; + + class EepromState { + int seen; + int n_pad_samples; + double ground_pres; + int boost_tick; + int sensor_tick; + + EepromState() { + seen = 0; + n_pad_samples = 0; + ground_pres = 0.0; + } + } + + void update_state(AltosRecordMini state, AltosEepromMini record, EepromState eeprom) { + state.tick = record.tick; + switch (record.cmd) { + case AltosLib.AO_LOG_FLIGHT: + eeprom.seen |= seen_flight; + state.ground_pres = record.ground_pres(); + state.flight_pres = state.ground_pres; + state.flight = record.data16(0); + eeprom.boost_tick = record.tick; + break; + case AltosLib.AO_LOG_SENSOR: + baro.set(record.pres(), record.temp()); + state.pres = baro.pa; + state.temp = baro.cc; + state.sense_m = record.sense_m(); + state.sense_a = record.sense_a(); + state.v_batt = record.v_batt(); + if (state.state < AltosLib.ao_flight_boost) { + eeprom.n_pad_samples++; + eeprom.ground_pres += state.pres; + state.ground_pres = (int) (eeprom.ground_pres / eeprom.n_pad_samples); + state.flight_pres = state.ground_pres; + } else { + state.flight_pres = (state.flight_pres * 15 + state.pres) / 16; + } + if ((eeprom.seen & seen_sensor) == 0) + eeprom.sensor_tick = record.tick - 1; + eeprom.seen |= seen_sensor; + eeprom.sensor_tick = record.tick; + break; + case AltosLib.AO_LOG_STATE: + state.state = record.state(); + break; + case AltosLib.AO_LOG_CONFIG_VERSION: + break; + case AltosLib.AO_LOG_MAIN_DEPLOY: + break; + case AltosLib.AO_LOG_APOGEE_DELAY: + break; + case AltosLib.AO_LOG_RADIO_CHANNEL: + break; + case AltosLib.AO_LOG_CALLSIGN: + state.callsign = record.data; + break; + case AltosLib.AO_LOG_RADIO_CAL: + break; + case AltosLib.AO_LOG_MANUFACTURER: + break; + case AltosLib.AO_LOG_PRODUCT: + break; + case AltosLib.AO_LOG_SERIAL_NUMBER: + state.serial = record.config_a; + break; + case AltosLib.AO_LOG_SOFTWARE_VERSION: + break; + case AltosLib.AO_LOG_BARO_RESERVED: + baro.reserved = record.config_a; + break; + case AltosLib.AO_LOG_BARO_SENS: + baro.sens =record.config_a; + break; + case AltosLib.AO_LOG_BARO_OFF: + baro.off =record.config_a; + break; + case AltosLib.AO_LOG_BARO_TCS: + baro.tcs =record.config_a; + break; + case AltosLib.AO_LOG_BARO_TCO: + baro.tco =record.config_a; + break; + case AltosLib.AO_LOG_BARO_TREF: + baro.tref =record.config_a; + break; + case AltosLib.AO_LOG_BARO_TEMPSENS: + baro.tempsens =record.config_a; + break; + case AltosLib.AO_LOG_BARO_CRC: + baro.crc =record.config_a; + break; + } + state.seen |= eeprom.seen; + } + + LinkedList make_list() { + LinkedList list = new LinkedList(); + Iterator iterator = records.iterator(); + AltosOrderedMiniRecord record = null; + AltosRecordMini state = new AltosRecordMini(); + //boolean last_reported = false; + EepromState eeprom = new EepromState(); + + state.state = AltosLib.ao_flight_pad; + + /* Pull in static data from the flight records */ + if (flight_record != null) + update_state(state, flight_record, eeprom); + + while (iterator.hasNext()) { + record = iterator.next(); + if ((eeprom.seen & seen_basic) == seen_basic && record.tick != state.tick) { + AltosRecordMini r = state.clone(); + r.time = (r.tick - eeprom.boost_tick) / 100.0; + list.add(r); + } + update_state(state, record, eeprom); + } + AltosRecordMini r = state.clone(); + r.time = (r.tick - eeprom.boost_tick) / 100.0; + list.add(r); + return list; + } + + public Iterator iterator() { + if (list == null) + list = make_list(); + return list.iterator(); + } + + public boolean has_gps() { return has_gps; } + public boolean has_accel() { return has_accel; } + public boolean has_ignite() { return has_ignite; } + + public void write_comments(PrintStream out) { + Iterator iterator = records.iterator(); + out.printf("# Comments\n"); + while (iterator.hasNext()) { + AltosOrderedMiniRecord record = iterator.next(); + switch (record.cmd) { + case AltosLib.AO_LOG_CONFIG_VERSION: + out.printf("# Config version: %s\n", record.data); + break; + case AltosLib.AO_LOG_MAIN_DEPLOY: + out.printf("# Main deploy: %s\n", record.config_a); + break; + case AltosLib.AO_LOG_APOGEE_DELAY: + out.printf("# Apogee delay: %s\n", record.config_a); + break; + case AltosLib.AO_LOG_RADIO_CHANNEL: + out.printf("# Radio channel: %s\n", record.config_a); + break; + case AltosLib.AO_LOG_CALLSIGN: + out.printf("# Callsign: %s\n", record.data); + break; + case AltosLib.AO_LOG_ACCEL_CAL: + out.printf ("# Accel cal: %d %d\n", record.config_a, record.config_b); + break; + case AltosLib.AO_LOG_RADIO_CAL: + out.printf ("# Radio cal: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_MAX_FLIGHT_LOG: + out.printf ("# Max flight log: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_MANUFACTURER: + out.printf ("# Manufacturer: %s\n", record.data); + break; + case AltosLib.AO_LOG_PRODUCT: + out.printf ("# Product: %s\n", record.data); + break; + case AltosLib.AO_LOG_SERIAL_NUMBER: + out.printf ("# Serial number: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_SOFTWARE_VERSION: + out.printf ("# Software version: %s\n", record.data); + break; + case AltosLib.AO_LOG_BARO_RESERVED: + out.printf ("# Baro reserved: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_SENS: + out.printf ("# Baro sens: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_OFF: + out.printf ("# Baro off: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_TCS: + out.printf ("# Baro tcs: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_TCO: + out.printf ("# Baro tco: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_TREF: + out.printf ("# Baro tref: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_TEMPSENS: + out.printf ("# Baro tempsens: %d\n", record.config_a); + break; + case AltosLib.AO_LOG_BARO_CRC: + out.printf ("# Baro crc: %d\n", record.config_a); + break; + } + } + } + + /* + * Read the whole file, dumping records into a RB tree so + * we can enumerate them in time order -- the eeprom data + * are sometimes out of order + */ + public AltosEepromMiniIterable (FileInputStream input) { + records = new TreeSet(); + + AltosOrderedMiniRecord last_gps_time = null; + + baro = new AltosMs5607(); + + int index = 0; + int prev_tick = 0; + boolean prev_tick_valid = false; + boolean missing_time = false; + + try { + for (;;) { + String line = AltosLib.gets(input); + if (line == null) + break; + AltosOrderedMiniRecord record = new AltosOrderedMiniRecord(line, index++, prev_tick, prev_tick_valid); + if (record.cmd == AltosLib.AO_LOG_INVALID) + continue; + prev_tick = record.tick; + if (record.cmd < AltosLib.AO_LOG_CONFIG_VERSION) + prev_tick_valid = true; + if (record.cmd == AltosLib.AO_LOG_FLIGHT) { + flight_record = record; + continue; + } + + records.add(record); + + /* Bail after reading the 'landed' record; we're all done */ + if (record.cmd == AltosLib.AO_LOG_STATE && + record.state() == AltosLib.ao_flight_landed) + break; + } + } catch (IOException io) { + } catch (ParseException pe) { + } + try { + input.close(); + } catch (IOException ie) { + } + } +} diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index 25d17e72..a629260b 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -216,6 +216,7 @@ public class AltosLib { public static final int AO_LOG_FORMAT_TELEMETRY = 3; public static final int AO_LOG_FORMAT_TELESCIENCE = 4; public static final int AO_LOG_FORMAT_TELEMEGA = 5; + public static final int AO_LOG_FORMAT_MINI = 6; public static final int AO_LOG_FORMAT_NONE = 127; public static boolean isspace(int c) { diff --git a/altoslib/AltosOrderedMiniRecord.java b/altoslib/AltosOrderedMiniRecord.java new file mode 100644 index 00000000..96888941 --- /dev/null +++ b/altoslib/AltosOrderedMiniRecord.java @@ -0,0 +1,52 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +import java.text.ParseException; + +/* + * AltosRecords with an index field so they can be sorted by tick while preserving + * the original ordering for elements with matching ticks + */ +class AltosOrderedMiniRecord extends AltosEepromMini implements Comparable { + + public int index; + + public AltosOrderedMiniRecord(String line, int in_index, int prev_tick, boolean prev_tick_valid) + throws ParseException { + super(line); + if (prev_tick_valid) { + tick |= (prev_tick & ~0xffff); + if (tick < prev_tick) { + if (prev_tick - tick > 0x8000) + tick += 0x10000; + } else { + if (tick - prev_tick > 0x8000) + tick -= 0x10000; + } + } + index = in_index; + } + + public int compareTo(AltosOrderedMiniRecord o) { + int tick_diff = tick - o.tick; + if (tick_diff != 0) + return tick_diff; + return index - o.index; + } +} diff --git a/altoslib/AltosRecordMini.java b/altoslib/AltosRecordMini.java new file mode 100644 index 00000000..253f3804 --- /dev/null +++ b/altoslib/AltosRecordMini.java @@ -0,0 +1,129 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +public class AltosRecordMini extends AltosRecord { + + /* Sensor values */ + public int pres; + public int temp; + + public int sense_a; + public int sense_m; + public int v_batt; + + public int ground_pres; + + public int flight_accel; + public int flight_vel; + public int flight_pres; + + static double adc(int raw) { + return raw / 4095.0; + } + + public double pressure() { + if (pres != MISSING) + return pres; + return MISSING; + } + + public double temperature() { + if (temp != MISSING) + return temp; + return MISSING; + } + + public double ground_pressure() { + if (ground_pres != MISSING) + return ground_pres; + return MISSING; + } + + public double battery_voltage() { + if (v_batt != MISSING) + return 3.3 * adc(v_batt) * (15.0 + 27.0) / 27.0; + return MISSING; + } + + static double pyro(int raw) { + if (raw != MISSING) + return 3.3 * adc(raw) * (100.0 + 27.0) / 27.0; + return MISSING; + } + + public double main_voltage() { + return pyro(sense_m); + } + + public double apogee_voltage() { + return pyro(sense_a); + } + + public void copy (AltosRecordMini old) { + super.copy(old); + + pres = old.pres; + temp = old.temp; + + sense_a = old.sense_a; + sense_m = old.sense_m; + v_batt = old.v_batt; + + ground_pres = old.ground_pres; + + flight_accel = old.flight_accel; + flight_vel = old.flight_vel; + flight_pres = old.flight_pres; + } + + + + public AltosRecordMini clone() { + return new AltosRecordMini(this); + } + + void make_missing() { + + pres = MISSING; + + sense_a = MISSING; + sense_m = MISSING; + v_batt = MISSING; + + ground_pres = MISSING; + + flight_accel = 0; + flight_vel = 0; + flight_pres = 0; + } + + public AltosRecordMini(AltosRecord old) { + super.copy(old); + make_missing(); + } + + public AltosRecordMini(AltosRecordMini old) { + copy(old); + } + + public AltosRecordMini() { + super(); + make_missing(); + } +} diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 18b028d6..8c1cf2ad 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -24,6 +24,8 @@ altoslib_JAVA = \ AltosEepromMegaIterable.java \ AltosEepromRecord.java \ AltosEepromTeleScience.java \ + AltosEepromMini.java \ + AltosEepromMiniIterable.java \ AltosFile.java \ AltosFlash.java \ AltosFlashListener.java \ @@ -47,6 +49,7 @@ altoslib_JAVA = \ AltosMs5607Query.java \ AltosOrderedRecord.java \ AltosOrderedMegaRecord.java \ + AltosOrderedMiniRecord.java \ AltosParse.java \ AltosPreferences.java \ AltosPreferencesBackend.java \ @@ -56,6 +59,7 @@ altoslib_JAVA = \ AltosRecordNone.java \ AltosRecordTM.java \ AltosRecordMM.java \ + AltosRecordMini.java \ AltosReplayReader.java \ AltosRomconfig.java \ AltosSensorMM.java \ diff --git a/altosui/AltosDataChooser.java b/altosui/AltosDataChooser.java index f914f138..c7b561d5 100644 --- a/altosui/AltosDataChooser.java +++ b/altosui/AltosDataChooser.java @@ -55,6 +55,9 @@ public class AltosDataChooser extends JFileChooser { } else if (filename.endsWith("mega")) { FileInputStream in = new FileInputStream(file); return new AltosEepromMegaIterable(in); + } else if (filename.endsWith("mini")) { + FileInputStream in = new FileInputStream(file); + return new AltosEepromMiniIterable(in); } else { throw new FileNotFoundException(); } @@ -77,8 +80,10 @@ public class AltosDataChooser extends JFileChooser { "telem")); setFileFilter(new FileNameExtensionFilter("TeleMega eeprom file", "mega")); + setFileFilter(new FileNameExtensionFilter("EasyMini eeprom file", + "mini")); setFileFilter(new FileNameExtensionFilter("Flight data file", - "telem", "eeprom", "mega")); + "telem", "eeprom", "mega", "mini")); setCurrentDirectory(AltosUIPreferences.logdir()); } } diff --git a/altosui/AltosEepromDownload.java b/altosui/AltosEepromDownload.java index a0523b58..53d5433f 100644 --- a/altosui/AltosEepromDownload.java +++ b/altosui/AltosEepromDownload.java @@ -302,6 +302,53 @@ public class AltosEepromDownload implements Runnable { CheckFile(false); } + void LogMini(AltosEepromMini r) throws IOException { + if (r.cmd != Altos.AO_LOG_INVALID) { + String log_line = String.format("%c %4x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x\n", + r.cmd, r.tick, + r.data8[0], r.data8[1], r.data8[2], r.data8[3], + r.data8[4], r.data8[5], r.data8[6], r.data8[7], + r.data8[8], r.data8[9], r.data8[10], r.data8[11]); + if (eeprom_file != null) + eeprom_file.write(log_line); + else + eeprom_pending.add(log_line); + } + } + + void CaptureMini(AltosEepromChunk eechunk) throws IOException { + boolean any_valid = false; + + extension = "mini"; + set_serial(flights.config_data.serial); + for (int i = 0; i < AltosEepromChunk.chunk_size && !done; i += AltosEepromMini.record_length) { + try { + AltosEepromMini r = new AltosEepromMini(eechunk, i); + if (r.cmd == Altos.AO_LOG_FLIGHT) + set_flight(r.data16(0)); + + /* Monitor state transitions to update display */ + if (r.cmd == Altos.AO_LOG_STATE && r.data16(0) <= Altos.ao_flight_landed) { + state = r.data16(0); + if (state > Altos.ao_flight_pad) + want_file = true; + } + + if (r.cmd == Altos.AO_LOG_STATE && r.data16(0) == Altos.ao_flight_landed) + done = true; + any_valid = true; + LogMini(r); + } catch (ParseException pe) { + if (parse_exception == null) + parse_exception = pe; + } + } + if (!any_valid) + done = true; + + CheckFile(false); + } + void CaptureTelemetry(AltosEepromChunk eechunk) throws IOException { } @@ -369,6 +416,9 @@ public class AltosEepromDownload implements Runnable { case AltosLib.AO_LOG_FORMAT_TELEMEGA: extension = "mega"; CaptureMega(eechunk); + case AltosLib.AO_LOG_FORMAT_MINI: + extension = "mini"; + CaptureMini(eechunk); } } CheckFile(true); diff --git a/altosui/AltosLanded.java b/altosui/AltosLanded.java index 1d209bda..9dab52c4 100644 --- a/altosui/AltosLanded.java +++ b/altosui/AltosLanded.java @@ -253,6 +253,9 @@ public class AltosLanded extends JComponent implements AltosFlightDisplay, Actio } else if (filename.endsWith("mega")) { FileInputStream in = new FileInputStream(file); records = new AltosEepromMegaIterable(in); + } else if (filename.endsWith("mini")) { + FileInputStream in = new FileInputStream(file); + records = new AltosEepromMiniIterable(in); } else { throw new FileNotFoundException(filename); } diff --git a/altosui/AltosUI.java b/altosui/AltosUI.java index 9f8f6dda..4362e36c 100644 --- a/altosui/AltosUI.java +++ b/altosui/AltosUI.java @@ -354,6 +354,8 @@ public class AltosUI extends AltosUIFrame { return new AltosEepromIterable(in); else if (file.getName().endsWith("mega")) return new AltosEepromMegaIterable(in); + else if (file.getName().endsWith("mini")) + return new AltosEepromMiniIterable(in); else return new AltosTelemetryIterable(in); } catch (FileNotFoundException fe) { @@ -441,6 +443,8 @@ public class AltosUI extends AltosUIFrame { recs = new AltosEepromIterable(in); } else if (file.getName().endsWith("mega")) { recs = new AltosEepromMegaIterable(in); + } else if (file.getName().endsWith("mini")) { + recs = new AltosEepromMiniIterable(in); } else { recs = new AltosTelemetryIterable(in); } diff --git a/src/core/ao_log.h b/src/core/ao_log.h index a68a40dd..95b37649 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -44,6 +44,7 @@ extern __pdata enum ao_flight_state ao_log_state; #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_MINI 6 /* 16-byte MS5607 baro only */ #define AO_LOG_FORMAT_NONE 127 /* No log at all */ extern __code uint8_t ao_log_format; @@ -261,6 +262,40 @@ struct ao_log_mega { } u; }; +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 */ + +static inline void +ao_log_pack24(uint8_t *dst, uint32_t value) { + dst[0] = value; + dst[1] = value >> 8; + dst[2] = value >> 16; +} + /* Write a record to the eeprom log */ uint8_t ao_log_data(__xdata struct ao_log_record *log) __reentrant; @@ -268,6 +303,9 @@ 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_mini(__xdata struct ao_log_mini *log) __reentrant; + void ao_log_flush(void); diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c new file mode 100644 index 00000000..1273b0e3 --- /dev/null +++ b/src/core/ao_log_mini.c @@ -0,0 +1,163 @@ +/* + * 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_MINI; + +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, 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; + 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_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; + 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/easymini-v0.1/Makefile b/src/easymini-v0.1/Makefile index c4e60ada..612cc472 100644 --- a/src/easymini-v0.1/Makefile +++ b/src/easymini-v0.1/Makefile @@ -34,7 +34,7 @@ ALTOS_SRC = \ ao_led_lpc.c \ ao_task.c \ ao_log.c \ - ao_log_tiny.c \ + ao_log_mini.c \ ao_cmd.c \ ao_config.c \ ao_timer_lpc.c \ diff --git a/src/easymini-v0.1/ao_pins.h b/src/easymini-v0.1/ao_pins.h index 4102c21d..d4fbe7a1 100644 --- a/src/easymini-v0.1/ao_pins.h +++ b/src/easymini-v0.1/ao_pins.h @@ -106,7 +106,7 @@ #define AO_ADC_2 1 struct ao_adc { - int16_t sense_d; + int16_t sense_a; int16_t sense_m; int16_t v_batt; }; @@ -126,6 +126,9 @@ struct ao_adc { #define AO_IGNITER_MAIN_PIN 3 #define AO_IGNITER_SET_MAIN(v) ao_gpio_set(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, v) +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) + #define AO_ADC_DUMP(p) \ - printf("tick: %5u drogue: %5d main: %5d batt: %5d\n", \ - (p)->tick, (p)->adc.sense_d, (p)->adc.sense_m, (p)->adc.v_batt) + printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \ + (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt) -- cgit v1.2.3 From 85eb75c3251d8e141d7269fc7ffa6197174ea8c3 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 21 May 2013 11:30:44 -0700 Subject: altos: Can't use inline functions because SDCC doesn't do that Sigh. Signed-off-by: Keith Packard --- src/core/ao_log.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_log.h b/src/core/ao_log.h index 95b37649..e1461a14 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -289,12 +289,11 @@ struct ao_log_mini { } u; /* 16 */ }; /* 16 */ -static inline void -ao_log_pack24(uint8_t *dst, uint32_t value) { - dst[0] = value; - dst[1] = value >> 8; - dst[2] = value >> 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 -- cgit v1.2.3 From 277577fecc71e3c52b823938f396cf42be403ebe Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 26 May 2013 19:01:58 -0600 Subject: altos: Add pyro code testing to ao_flight_test for TeleMega This parses the pyro settings and signals when the pyro channels are fired in the output. Signed-off-by: Keith Packard --- src/core/ao_pyro.c | 120 +++++++++++++++++++++++++++------------------- src/test/Makefile | 2 +- src/test/ao_flight_test.c | 57 ++++++++++++++++++++++ 3 files changed, 130 insertions(+), 49 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index aac8fda5..b655eaca 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -15,10 +15,12 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_FLIGHT_TEST #include -#include #include #include +#endif +#include #if IS_COMPANION #include @@ -130,12 +132,13 @@ ao_pyro_ready(struct ao_pyro *pyro) return TRUE; } +#ifndef AO_FLIGHT_TEST #define ao_pyro_fire_port(port, bit, pin) do { \ ao_gpio_set(port, bit, pin, 1); \ ao_delay(AO_MS_TO_TICKS(50)); \ ao_gpio_set(port, bit, pin, 0); \ } while (0) - +#endif static void ao_pyro_fire(uint8_t p) @@ -172,66 +175,58 @@ ao_pyro_fire(uint8_t p) uint8_t ao_pyro_wakeup; -static void -ao_pyro(void) +static uint8_t +ao_pyro_check(void) { - uint8_t p, any_waiting; struct ao_pyro *pyro; + uint8_t p, any_waiting; + + any_waiting = 0; + for (p = 0; p < AO_PYRO_NUM; p++) { + pyro = &ao_config.pyro[p]; - 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(); - 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 igniters which have already fired - */ - if (pyro->fired) - continue; + /* Ignore disabled igniters + */ + if (!pyro->flags) + continue; - /* Ignore disabled igniters - */ - if (!pyro->flags) + any_waiting = 1; + /* Check pyro state to see if it should fire + */ + if (!pyro->delay_done) { + if (!ao_pyro_ready(pyro)) continue; - any_waiting = 1; - /* Check pyro state to see if it shoule fire + /* If there's a delay set, then remember when + * it expires */ - 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; - } - - /* 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; + if (pyro->flags & ao_pyro_delay) { + pyro->delay_done = ao_time() + pyro->delay; + if (!pyro->delay_done) + pyro->delay_done = 1; } + } - ao_pyro_fire(p); + /* 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; } - if (!any_waiting) - break; + + ao_pyro_fire(p); + pyro->fired = 1; + ao_pyro_fired |= (1 << p); } - ao_exit(); + return any_waiting; } -__xdata struct ao_task ao_pyro_task; - #define NO_VALUE 0xff #define AO_PYRO_NAME_LEN 3 @@ -283,6 +278,34 @@ const struct { { "", 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) { @@ -449,3 +472,4 @@ ao_pyro_init(void) ao_cmd_register(&ao_pyro_cmds[0]); ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); } +#endif diff --git a/src/test/Makefile b/src/test/Makefile index 8032a163..3c9ac9c6 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -29,7 +29,7 @@ ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalm ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c -ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) +ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c ao_pyro.c ao_pyro.h $(INCS) cc -DTELEMEGA=1 $(CFLAGS) -o $@ $< -lm ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 99bed7ee..6e53d8e1 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -137,6 +138,18 @@ int tick_offset; static int32_t ao_k_height; +int16_t +ao_time(void) +{ + return ao_data_static.tick; +} + +void +ao_delay(int16_t interval) +{ + return; +} + void ao_ignite(enum ao_igniter igniter) { @@ -200,6 +213,8 @@ struct ao_cmds { #include struct ao_ms5607_prom ms5607_prom; #include "ao_ms5607_convert.c" +#define AO_PYRO_NUM 4 +#include #else #include "ao_convert.c" #endif @@ -210,6 +225,9 @@ struct ao_config { int16_t accel_minus_g; uint8_t pad_orientation; uint16_t apogee_lockout; +#if TELEMEGA + struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ +#endif }; #define AO_PAD_ORIENTATION_ANTENNA_UP 0 @@ -246,6 +264,24 @@ uint16_t prev_tick; #include "ao_sqrt.c" #include "ao_sample.c" #include "ao_flight.c" +#if TELEMEGA +#define AO_PYRO_NUM 4 + +#define AO_PYRO_0 0 +#define AO_PYRO_1 1 +#define AO_PYRO_2 2 +#define AO_PYRO_3 3 + +static void +ao_pyro_tell(int pin) +{ + printf ("fire pyro %d\n", pin); +} + +#define ao_pyro_fire_port(port,bit,pin) ao_pyro_tell(pin) + +#include "ao_pyro.c" +#endif #define to_double(f) ((f) / 65536.0) @@ -771,6 +807,10 @@ ao_sleep(void *wchan) char *words[64]; int nword; +#if TELEMEGA + if (ao_flight_state >= ao_flight_boost && ao_flight_state < ao_flight_landed) + ao_pyro_check(); +#endif for (;;) { if (ao_records_read > 2 && ao_flight_state == ao_flight_startup) { @@ -883,6 +923,23 @@ ao_sleep(void *wchan) else if (strcmp(words[1], "crc:") == 0) ms5607_prom.crc = strtoul(words[2], NULL, 10); continue; + } else if (nword >= 3 && strcmp(words[0], "Pyro") == 0) { + int p = strtoul(words[1], NULL, 10); + int i, j; + struct ao_pyro *pyro = &ao_config.pyro[p]; + + for (i = 2; i < nword; i++) { + for (j = 0; j < NUM_PYRO_VALUES; j++) + if (!strcmp (words[2], ao_pyro_values[j].name)) + break; + if (j == NUM_PYRO_VALUES) + continue; + pyro->flags |= ao_pyro_values[j].flag; + if (ao_pyro_values[j].offset != NO_VALUE && i + 1 < nword) { + int16_t val = strtoul(words[++i], NULL, 10); + *((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val; + } + } } #else if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) { -- cgit v1.2.3 From 62547a042d042fadec652c5081f96816a8e66970 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 26 May 2013 19:03:12 -0600 Subject: altos,altosui: Add pyro state logging for TeleMega Only in the log file (no obvious space in the telem packets), but at least we should be able to check for pyro failures. Signed-off-by: Keith Packard --- altoslib/AltosEepromMega.java | 1 + src/core/ao_log.h | 3 ++- src/core/ao_log_mega.c | 1 + src/core/ao_pyro.c | 2 ++ src/core/ao_pyro.h | 2 ++ 5 files changed, 8 insertions(+), 1 deletion(-) (limited to 'src/core') diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java index b077e26c..0804c392 100644 --- a/altoslib/AltosEepromMega.java +++ b/altoslib/AltosEepromMega.java @@ -73,6 +73,7 @@ public class AltosEepromMega { public int v_pbatt() { return data16(2); } public int nsense() { return data16(4); } public int sense(int i) { return data16(6 + i * 2); } + public int pyro() { return data16(26); } /* AO_LOG_GPS_TIME elements */ public int latitude() { return data32(0); } diff --git a/src/core/ao_log.h b/src/core/ao_log.h index e1461a14..dce12f02 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -236,7 +236,8 @@ struct ao_log_mega { int16_t v_pbatt; /* 6 */ int16_t n_sense; /* 8 */ int16_t sense[10]; /* 10 */ - } volt; /* 30 */ + uint16_t pyro; /* 30 */ + } volt; /* 32 */ /* AO_LOG_GPS_TIME */ struct { int32_t latitude; /* 4 */ diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c index abf953a6..0db1119c 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -151,6 +151,7 @@ ao_log(void) 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.pyro = ao_pyro_fired; ao_log_mega(&log); next_other = log.tick + AO_OTHER_INTERVAL; } diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index b655eaca..b3cda656 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -33,6 +33,8 @@ #define ao_lowbit(x) ((x) & (-x)) +uint16_t ao_pyro_fired; + /* * Given a pyro structure, figure out * if the current flight state satisfies all diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h index cde850ad..1f838542 100644 --- a/src/core/ao_pyro.h +++ b/src/core/ao_pyro.h @@ -63,6 +63,8 @@ struct ao_pyro { extern uint8_t ao_pyro_wakeup; +extern uint16_t ao_pyro_fired; + void ao_pyro_set(void); -- cgit v1.2.3 From 956f4dff1cc521059434743624b1271fb92b96ae Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 26 May 2013 19:39:13 -0600 Subject: altos: Light pyro charges simultaneously if so configured Don't try to be nice to the battery, just let the pyro circuit deal with it and try to get all of the specified circuits going at the same time if they're configured to do so. Signed-off-by: Keith Packard --- src/core/ao_pyro.c | 49 ++++++++++++++++++++++++++++------------------- src/test/ao_flight_test.c | 6 ++---- 2 files changed, 31 insertions(+), 24 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index b3cda656..84f949dc 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -135,45 +135,38 @@ ao_pyro_ready(struct ao_pyro *pyro) } #ifndef AO_FLIGHT_TEST -#define ao_pyro_fire_port(port, bit, pin) do { \ - ao_gpio_set(port, bit, pin, 1); \ - ao_delay(AO_MS_TO_TICKS(50)); \ - ao_gpio_set(port, bit, pin, 0); \ - } while (0) -#endif - static void -ao_pyro_fire(uint8_t p) +ao_pyro_pin_set(uint8_t p, uint8_t v) { switch (p) { #if AO_PYRO_NUM > 0 - case 0: ao_pyro_fire_port(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0); break; + 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_pyro_fire_port(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1); break; + 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_pyro_fire_port(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2); break; + 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_pyro_fire_port(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3); break; + 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_pyro_fire_port(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4); break; + 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_pyro_fire_port(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5); break; + 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_pyro_fire_port(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6); break; + 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_pyro_fire_port(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7); break; + case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; #endif default: break; } - ao_delay(AO_MS_TO_TICKS(50)); } +#endif uint8_t ao_pyro_wakeup; @@ -182,6 +175,7 @@ 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++) { @@ -222,10 +216,25 @@ ao_pyro_check(void) continue; } - ao_pyro_fire(p); - pyro->fired = 1; - ao_pyro_fired |= (1 << p); + fire |= (1 << p); } + + if (fire) { + 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)); + } + return any_waiting; } diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 6e53d8e1..faf31aa7 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -273,13 +273,11 @@ uint16_t prev_tick; #define AO_PYRO_3 3 static void -ao_pyro_tell(int pin) +ao_pyro_pin_set(uint8_t pin, uint8_t value) { - printf ("fire pyro %d\n", pin); + printf ("set pyro %d %d\n", pin, value); } -#define ao_pyro_fire_port(port,bit,pin) ao_pyro_tell(pin) - #include "ao_pyro.c" #endif -- cgit v1.2.3 From 5ca472333a3587f0e47d54f5edc287494262ef98 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 26 May 2013 19:47:02 -0600 Subject: altos: write pyro fired to correct log field Signed-off-by: Keith Packard --- src/core/ao_log_mega.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c index 0db1119c..ee12b907 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -151,7 +151,7 @@ ao_log(void) 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.pyro = ao_pyro_fired; + log.u.volt.pyro = ao_pyro_fired; ao_log_mega(&log); next_other = log.tick + AO_OTHER_INTERVAL; } -- cgit v1.2.3 From 6f131e740477d29b6623fa336da79e53f765a55b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 26 May 2013 19:48:03 -0600 Subject: altos: Make manual pyro firing command work again Signed-off-by: Keith Packard --- src/core/ao_pyro.c | 39 +++++++++++++++++++++++---------------- 1 file changed, 23 insertions(+), 16 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index 84f949dc..39f40dfa 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -170,6 +170,26 @@ ao_pyro_pin_set(uint8_t p, uint8_t v) 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) { @@ -219,21 +239,8 @@ ao_pyro_check(void) fire |= (1 << p); } - if (fire) { - 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)); - } + if (fire) + ao_pyro_pins_fire(fire); return any_waiting; } @@ -444,7 +451,7 @@ ao_pyro_manual(void) ao_cmd_decimal(); if (ao_cmd_lex_i < 0 || AO_PYRO_NUM <= ao_cmd_lex_i) return; - ao_pyro_fire(ao_cmd_lex_i); + ao_pyro_pins_fire(1 << ao_cmd_lex_i); } -- cgit v1.2.3 From 3e8b72a9dc5b6c3a0f6132dc2dec04f8c08a1deb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 26 May 2013 22:38:56 -0600 Subject: altos: Add pyro operations to regular ignite commands Instead of having separate commands, just mix the two sets together. Signed-off-by: Keith Packard --- src/Makefile | 2 +- src/core/ao.h | 2 ++ src/core/ao_ignite.c | 45 ++++++++++++++++++++++++++++------- src/core/ao_pyro.c | 57 ++++++++++++++++++++++++++++++++------------- src/core/ao_pyro.h | 6 +++++ src/telemega-v0.1/ao_pins.h | 2 ++ src/telemega-v0.3/ao_pins.h | 1 + 7 files changed, 89 insertions(+), 26 deletions(-) (limited to 'src/core') diff --git a/src/Makefile b/src/Makefile index 7ffc52d6..ff26ac20 100644 --- a/src/Makefile +++ b/src/Makefile @@ -26,7 +26,7 @@ SDCCDIRS=\ spiradio-v0.1 AVRDIRS=\ - telescience-v0.1 telescience-pwm telepyro-v0.1 micropeak + telescience-v0.1 telescience-pwm micropeak ARMDIRS=\ telemega-v0.1 telemega-v0.1/flash-loader \ diff --git a/src/core/ao.h b/src/core/ao.h index 7f344736..0886260f 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -703,6 +703,8 @@ struct ao_ignition { uint8_t firing; }; +extern __code char * __code ao_igniter_status_names[]; + extern __xdata struct ao_ignition ao_ignition[2]; enum ao_igniter_status diff --git a/src/core/ao_ignite.c b/src/core/ao_ignite.c index 74bd0c5a..9f2ec0a7 100644 --- a/src/core/ao_ignite.c +++ b/src/core/ao_ignite.c @@ -17,7 +17,11 @@ #include "ao.h" #include +#if AO_PYRO_NUM +#include +#endif +#if HAS_IGNITE __xdata struct ao_ignition ao_ignition[2]; void @@ -150,6 +154,8 @@ ao_igniter(void) } } +#endif + void ao_ignite_manual(void) { @@ -157,33 +163,50 @@ ao_ignite_manual(void) if (!ao_match_word("DoIt")) return; ao_cmd_white(); - if (ao_cmd_lex_c == 'm') { - if(ao_match_word("main")) - ao_igniter_fire(ao_igniter_main); - } else { - if(ao_match_word("drogue")) - ao_igniter_fire(ao_igniter_drogue); +#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; } -static __code char * __code igniter_status_names[] = { +__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, - igniter_status_names[status]); + 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[] = { @@ -192,6 +215,7 @@ __code struct ao_cmds ao_ignite_cmds[] = { { 0, NULL }, }; +#if HAS_IGNITE __xdata struct ao_task ao_igniter_task; void @@ -200,11 +224,14 @@ 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_cmd_register(&ao_ignite_cmds[0]); ao_add_task(&ao_igniter_task, ao_igniter, "igniter"); +#endif + ao_cmd_register(&ao_ignite_cmds[0]); } diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index 39f40dfa..531e1f50 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -33,6 +33,40 @@ #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; /* @@ -441,25 +475,17 @@ ao_pyro_set(void) _ao_config_edit_finish(); } -static void -ao_pyro_manual(void) +void +ao_pyro_manual(uint8_t p) { - ao_cmd_white(); - if (!ao_match_word("DoIt")) + printf ("ao_pyro_manual %d\n", p); + if (p >= AO_PYRO_NUM) { + ao_cmd_status = ao_cmd_syntax_error; return; - ao_cmd_white(); - ao_cmd_decimal(); - if (ao_cmd_lex_i < 0 || AO_PYRO_NUM <= ao_cmd_lex_i) - return; - ao_pyro_pins_fire(1 << ao_cmd_lex_i); - + } + ao_pyro_pins_fire(1 << p); } -const struct ao_cmds ao_pyro_cmds[] = { - { ao_pyro_manual, "P DoIt \0Fire igniter" }, - { 0, NULL } -}; - void ao_pyro_init(void) { @@ -487,7 +513,6 @@ ao_pyro_init(void) #if AO_PYRO_NUM > 7 ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); #endif - ao_cmd_register(&ao_pyro_cmds[0]); ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); } #endif diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h index 1f838542..0c5642d6 100644 --- a/src/core/ao_pyro.h +++ b/src/core/ao_pyro.h @@ -74,4 +74,10 @@ 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/telemega-v0.1/ao_pins.h b/src/telemega-v0.1/ao_pins.h index 4c645871..78e887c3 100644 --- a/src/telemega-v0.1/ao_pins.h +++ b/src/telemega-v0.1/ao_pins.h @@ -146,6 +146,8 @@ /* Number of general purpose pyro channels available */ #define AO_PYRO_NUM 4 +#define AO_SENSE_PYRO(a,p) ((a)->adc.sense[(p) + 2]) + #define AO_IGNITER_SET_DROGUE(v) stm_gpio_set(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, v) #define AO_IGNITER_SET_MAIN(v) stm_gpio_set(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, v) diff --git a/src/telemega-v0.3/ao_pins.h b/src/telemega-v0.3/ao_pins.h index bace5853..a5a9eaf4 100644 --- a/src/telemega-v0.3/ao_pins.h +++ b/src/telemega-v0.3/ao_pins.h @@ -121,6 +121,7 @@ #define HAS_IGNITE 1 #define HAS_IGNITE_REPORT 1 +#define AO_SENSE_PYRO(p,n) ((p)->adc.sense[n]) #define AO_SENSE_DROGUE(p) ((p)->adc.sense[4]) #define AO_SENSE_MAIN(p) ((p)->adc.sense[5]) #define AO_IGNITER_CLOSED 400 -- cgit v1.2.3 From 72b6c699d355fcd41addb9919d846e63105b9db7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 13 May 2013 22:34:19 -0700 Subject: altos: Add debounce helper. Use in button and quadrature drivers for TeleLCO Signed-off-by: Keith Packard --- src/core/ao_debounce.h | 52 ++++++++++++ src/drivers/ao_button.c | 82 ++++++++++++------- src/drivers/ao_event.c | 17 +--- src/drivers/ao_event.h | 6 +- src/drivers/ao_quadrature.c | 108 +++++++++++++++++-------- src/stm-demo/Makefile | 5 +- src/stm-demo/ao_demo.c | 4 +- src/stm/ao_debounce.c | 187 ++++++++++++++++++++++++++++++++++++++++++++ src/telelco-v0.2/Makefile | 2 + src/telelco-v0.2/ao_lco.c | 22 ++---- 10 files changed, 389 insertions(+), 96 deletions(-) create mode 100644 src/core/ao_debounce.h create mode 100644 src/stm/ao_debounce.c (limited to 'src/core') diff --git a/src/core/ao_debounce.h b/src/core/ao_debounce.h new file mode 100644 index 00000000..ebe290e1 --- /dev/null +++ b/src/core/ao_debounce.h @@ -0,0 +1,52 @@ +/* + * 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 */ + int8_t hold; + + /* last value reported to app; don't report it twice */ + uint8_t value; + + /* current count of intervals pin value has been stable */ + int8_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); +}; + +void +_ao_debounce_start(struct ao_debounce *debounce); + +void +_ao_debounce_stop(struct ao_debounce *debounce); + +void +ao_debounce_init(void); + +#endif /* _AO_DEBOUNCE_H_ */ diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c index a507c909..7b1fb530 100644 --- a/src/drivers/ao_button.c +++ b/src/drivers/ao_button.c @@ -18,6 +18,7 @@ #include #include #include +#include #if AO_EVENT #include #define ao_button_queue(b,v) ao_event_put_isr(AO_EVENT_BUTTON, b, v) @@ -25,55 +26,70 @@ #define ao_button_queue(b,v) #endif -static uint8_t ao_button[AO_BUTTON_COUNT]; -static AO_TICK_TYPE ao_button_time[AO_BUTTON_COUNT]; +#define AO_BUTTON_DEBOUNCE_HOLD 10 -#define AO_DEBOUNCE AO_MS_TO_TICKS(20) +static struct ao_debounce ao_button_debounce[AO_BUTTON_COUNT]; #define port(q) AO_BUTTON_ ## q ## _PORT #define bit(q) AO_BUTTON_ ## q #define pin(q) AO_BUTTON_ ## q ## _PIN -static void -ao_button_do(uint8_t b, uint8_t v) -{ - /* Debounce */ - if ((AO_TICK_SIGNED) (ao_tick_count - ao_button_time[b]) < AO_DEBOUNCE) - return; - - /* pins are inverted */ - v = !v; - if (ao_button[b] != v) { - ao_button[b] = v; - ao_button_time[b] = ao_tick_count; - ao_button_queue(b, v); - ao_wakeup(&ao_button[b]); - } -} +/* pins are inverted */ +#define ao_button_value(b) !ao_gpio_get(port(b), bit(b), pin(b)) -#define ao_button_update(b) ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b))) - -static void -ao_button_isr(void) +static uint8_t +_ao_button_get(struct ao_debounce *debounce) { + uint8_t b = debounce - ao_button_debounce; + + switch (b) { #if AO_BUTTON_COUNT > 0 - ao_button_update(0); + case 0: return ao_button_value(0); #endif #if AO_BUTTON_COUNT > 1 - ao_button_update(1); + case 1: return ao_button_value(1); #endif #if AO_BUTTON_COUNT > 2 - ao_button_update(2); + case 2: return ao_button_value(2); #endif #if AO_BUTTON_COUNT > 3 - ao_button_update(3); + case 3: return ao_button_value(3); #endif #if AO_BUTTON_COUNT > 4 - ao_button_update(4); + case 4: return ao_button_value(4); #endif + } +} + +static void +_ao_button_set(struct ao_debounce *debounce, uint8_t value) +{ + uint8_t b = debounce - ao_button_debounce; + + ao_button_queue(b, value); +} + + +#define ao_button_update(b) ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b))) + +static void +ao_button_debounce_init(struct ao_debounce *debounce) { + debounce->hold = AO_BUTTON_DEBOUNCE_HOLD; + debounce->_get = _ao_button_get; + debounce->_set = _ao_button_set; +} + +static void +ao_button_isr(void) +{ + uint8_t b; + + for (b = 0; b < AO_BUTTON_COUNT; b++) + _ao_debounce_start(&ao_button_debounce[b]); } #define init(b) do { \ + ao_button_debounce_init(&ao_button_debounce[b]); \ ao_enable_port(port(b)); \ \ ao_exti_setup(port(b), bit(b), \ @@ -91,4 +107,14 @@ ao_button_init(void) #if AO_BUTTON_COUNT > 1 init(1); #endif +#if AO_BUTTON_COUNT > 2 + init(2); +#endif +#if AO_BUTTON_COUNT > 3 + init(3); +#endif +#if AO_BUTTON_COUNT > 4 + init(4); +#endif + ao_debounce_init(); } diff --git a/src/drivers/ao_event.c b/src/drivers/ao_event.c index 440ef2de..c428125d 100644 --- a/src/drivers/ao_event.c +++ b/src/drivers/ao_event.c @@ -25,11 +25,6 @@ #define ao_event_queue_empty() (ao_event_queue_insert == ao_event_queue_remove) #define ao_event_queue_full() (ao_event_queue_next(ao_event_queue_insert) == ao_event_queue_remove) -/* - * Whether a sequence of events from the same device should be collapsed - */ -#define ao_event_can_collapse(type) ((type) == AO_EVENT_QUADRATURE) - struct ao_event ao_event_queue[AO_EVENT_QUEUE]; uint8_t ao_event_queue_insert; uint8_t ao_event_queue_remove; @@ -48,17 +43,9 @@ ao_event_get(struct ao_event *ev) /* called with interrupts disabled */ void -ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value) +ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value) { if (!ao_event_queue_full()) { - - if (ao_event_can_collapse(type) && !ao_event_queue_empty()) { - uint8_t prev = ao_event_queue_prev(ao_event_queue_insert); - - if (ao_event_queue[prev].type == type && - ao_event_queue[prev].unit == unit) - ao_event_queue_insert = prev; - } ao_event_queue[ao_event_queue_insert] = (struct ao_event) { .type = type, .unit = unit, @@ -71,7 +58,7 @@ ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value) } void -ao_event_put(uint8_t type, uint8_t unit, uint32_t value) +ao_event_put(uint8_t type, uint8_t unit, int32_t value) { ao_arch_critical(ao_event_put_isr(type, unit, value);); } diff --git a/src/drivers/ao_event.h b/src/drivers/ao_event.h index 25c49c35..ed9a7433 100644 --- a/src/drivers/ao_event.h +++ b/src/drivers/ao_event.h @@ -26,16 +26,16 @@ struct ao_event { uint8_t type; uint8_t unit; uint16_t tick; - uint32_t value; + int32_t value; }; uint8_t ao_event_get(struct ao_event *ev); void -ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value); +ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value); void -ao_event_put(uint8_t type, uint8_t unit, uint32_t value); +ao_event_put(uint8_t type, uint8_t unit, int32_t value); #endif /* _AO_EVENT_H_ */ diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index 6cc2467a..cfa58da6 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -18,12 +18,14 @@ #include #include #include -#if AO_EVENT +#include #include -#define ao_quadrature_queue(q) ao_event_put_isr(AO_EVENT_QUADRATURE, q, ao_quadrature_count[q]) -#else -#define ao_quadrature_queue(q) -#endif + +#define AO_QUADRATURE_DEBOUNCE_HOLD 3 + +static __xdata struct ao_debounce ao_quadrature_debounce[AO_QUADRATURE_COUNT]; + +#define debounce_id(d) ((d) - ao_quadrature_debounce) __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; @@ -35,41 +37,77 @@ static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; #define port(q) AO_QUADRATURE_ ## q ## _PORT #define bita(q) AO_QUADRATURE_ ## q ## _A #define bitb(q) AO_QUADRATURE_ ## q ## _B +#define pina(q) AO_QUADRATURE_ ## q ## _A ## _PIN +#define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN -#define ao_quadrature_update(q) do { \ - ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); \ - ao_quadrature_state[q] |= ao_gpio_get(port(q), bita(q), 0); \ - ao_quadrature_state[q] |= ao_gpio_get(port(q), bitb(q), 0) << 1; \ - } while (0) - +#define q_case(q) case q: v = (!ao_gpio_get(port(q), bita(q), pina(q))) | ((!ao_gpio_get(port(q), bitb(q), pinb(q))) << 1); break -static void -ao_quadrature_isr(void) -{ - uint8_t q; +uint8_t quad_raw[64]; +uint8_t quad_r; + +static uint8_t +_ao_quadrature_get(struct ao_debounce *debounce) { + uint8_t q = debounce_id(debounce); + uint8_t v = 0; + + switch (q) { #if AO_QUADRATURE_COUNT > 0 - ao_quadrature_update(0); + q_case(0); #endif #if AO_QUADRATURE_COUNT > 1 - ao_quadrature_update(1); + q_case(1); #endif + } + if (q == 0) { + quad_raw[quad_r] = v; + quad_r = (quad_r + 1) & 63; + } + return v; +} - for (q = 0; q < AO_QUADRATURE_COUNT; q++) { - switch (ao_quadrature_state[q]) { - case STATE(0, 1, 0, 0): - ao_quadrature_count[q]++; - break; - case STATE(1, 0, 0, 0): - ao_quadrature_count[q]--; - break; - default: - continue; - } - ao_quadrature_queue(q); - ao_wakeup(&ao_quadrature_count[q]); +static void +_ao_quadrature_queue(uint8_t q, int8_t step) +{ + ao_quadrature_count[q] += step; +#if AO_EVENT + ao_event_put_isr(AO_EVENT_QUADRATURE, q, step); +#endif + ao_wakeup(&ao_quadrature_count[q]); +} + +uint8_t quad_history[64]; +uint8_t quad_h; + +static void +_ao_quadrature_set(struct ao_debounce *debounce, uint8_t value) { + uint8_t q = debounce_id(debounce); + + ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); + ao_quadrature_state[q] |= value; + + if (q == 0) { + quad_history[quad_h] = ao_quadrature_state[0]; + quad_h = (quad_h + 1) & 63; + } + + switch (ao_quadrature_state[q]) { + case STATE(0, 1, 0, 0): + _ao_quadrature_queue(q, 1); + break; + case STATE(1, 0, 0, 0): + _ao_quadrature_queue(q, -1); + break; } } +static void +ao_quadrature_isr(void) +{ + uint8_t q; + for (q = 0; q < AO_QUADRATURE_COUNT; q++) + _ao_debounce_start(&ao_quadrature_debounce[q]); +} + int32_t ao_quadrature_poll(uint8_t q) { @@ -107,9 +145,16 @@ static const struct ao_cmds ao_quadrature_cmds[] = { { 0, NULL } }; +static void +ao_quadrature_debounce_init(struct ao_debounce *debounce) { + debounce->hold = AO_QUADRATURE_DEBOUNCE_HOLD; + debounce->_get = _ao_quadrature_get; + debounce->_set = _ao_quadrature_set; +} + #define init(q) do { \ ao_enable_port(port(q)); \ - \ + ao_quadrature_debounce_init(&ao_quadrature_debounce[q]); \ ao_exti_setup(port(q), bita(q), \ AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \ ao_quadrature_isr); \ @@ -124,6 +169,7 @@ static const struct ao_cmds ao_quadrature_cmds[] = { void ao_quadrature_init(void) { + ao_debounce_init(); #if AO_QUADRATURE_COUNT > 0 init(0); #endif diff --git a/src/stm-demo/Makefile b/src/stm-demo/Makefile index d1f825db..48fa07eb 100644 --- a/src/stm-demo/Makefile +++ b/src/stm-demo/Makefile @@ -36,10 +36,7 @@ ALTOS_SRC = \ ao_data.c \ ao_i2c_stm.c \ ao_usb_stm.c \ - ao_exti_stm.c \ - ao_event.c \ - ao_quadrature.c \ - ao_button.c + ao_exti_stm.c PRODUCT=StmDemo-v0.0 PRODUCT_DEF=-DSTM_DEMO diff --git a/src/stm-demo/ao_demo.c b/src/stm-demo/ao_demo.c index 5677cdf4..58cf651b 100644 --- a/src/stm-demo/ao_demo.c +++ b/src/stm-demo/ao_demo.c @@ -153,6 +153,7 @@ ao_temp (void) printf ("temp: %d\n", temp); } +#if 0 static void ao_event(void) { @@ -168,6 +169,7 @@ ao_event(void) } } +#endif __code struct ao_cmds ao_demo_cmds[] = { { ao_dma_test, "D\0DMA test" }, @@ -175,7 +177,7 @@ __code struct ao_cmds ao_demo_cmds[] = { { ao_spi_read, "R\0SPI read" }, { ao_i2c_write, "i\0I2C write" }, { ao_temp, "t\0Show temp" }, - { ao_event, "e\0Monitor event queue" }, +/* { ao_event, "e\0Monitor event queue" }, */ { 0, NULL } }; diff --git a/src/stm/ao_debounce.c b/src/stm/ao_debounce.c new file mode 100644 index 00000000..22cdf230 --- /dev/null +++ b/src/stm/ao_debounce.c @@ -0,0 +1,187 @@ +/* + * 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 + +static uint8_t ao_debounce_initialized; +static uint8_t ao_debounce_running; +static struct ao_debounce *ao_debounce; + +static void +ao_debounce_on(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (1 << STM_TIM67_CR1_CEN)); +} + +static void +ao_debounce_off(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (0 << STM_TIM67_CR1_CEN)); +} + +static void +_ao_debounce_set(struct ao_debounce *debounce, uint8_t value) +{ + if (value != debounce->value) { + debounce->value = value; + debounce->_set(debounce, value); + } + _ao_debounce_stop(debounce); +} + +/* + * Get the current value, set the result when we've + * reached the debounce count limit + */ +static void +_ao_debounce_check(struct ao_debounce *debounce) +{ + if (debounce->_get(debounce)) { + if (debounce->count < 0) + debounce->count = 0; + if (debounce->count < debounce->hold) { + if (++debounce->count == debounce->hold) + _ao_debounce_set(debounce, 1); + } + } else { + if (debounce->count > 0) + debounce->count = 0; + if (debounce->count > -debounce->hold) { + if (--debounce->count == -debounce->hold) + _ao_debounce_set(debounce, 0); + } + } +} + +/* + * Start monitoring one pin + */ +void +_ao_debounce_start(struct ao_debounce *debounce) +{ + if (debounce->running) + return; + 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); +} + +/* + * Stop monitoring one pin + */ +void +_ao_debounce_stop(struct ao_debounce *debounce) +{ + struct ao_debounce **prev; + if (!debounce->running) + return; + + 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(); +} + +void stm_tim6_isr(void) +{ + struct ao_debounce *debounce, *next; + if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) { + stm_tim6.sr = 0; + + /* Walk the current list, allowing the current + * object to be removed from the list + */ + for (debounce = ao_debounce; debounce; debounce = next) { + next = debounce->next; + _ao_debounce_check(debounce); + } + } +} + +/* + * According to the STM clock-configuration, timers run + * twice as fast as the APB1 clock *if* the APB1 prescaler + * is greater than 1. + */ + +#if AO_APB1_PRESCALER > 1 +#define TIMER_23467_SCALER 2 +#else +#define TIMER_23467_SCALER 1 +#endif + +#define TIMER_100kHz ((AO_PCLK1 * TIMER_23467_SCALER) / 100000) + +void +ao_debounce_init(void) +{ + if (ao_debounce_initialized) + return; + ao_debounce_initialized = 1; + + stm_nvic_set_enable(STM_ISR_TIM6_POS); + stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY); + + /* Turn on timer 6 */ + stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN); + + stm_tim6.psc = TIMER_100kHz; + stm_tim6.arr = 9; + stm_tim6.cnt = 0; + + /* Enable update interrupt */ + stm_tim6.dier = (1 << STM_TIM67_DIER_UIE); + + /* Poke timer to reload values */ + stm_tim6.egr |= (1 << STM_TIM67_EGR_UG); + + stm_tim6.cr2 = (STM_TIM67_CR2_MMS_RESET << STM_TIM67_CR2_MMS); + + /* And turn it off (for now) */ + ao_debounce_off(); +} diff --git a/src/telelco-v0.2/Makefile b/src/telelco-v0.2/Makefile index cc6e62c4..42a5a7ee 100644 --- a/src/telelco-v0.2/Makefile +++ b/src/telelco-v0.2/Makefile @@ -21,6 +21,7 @@ INC = \ ao_radio_spi.h \ ao_radio_cmac.h \ ao_cc1120_CC1120.h \ + ao_debounce.h \ stm32l.h # @@ -59,6 +60,7 @@ ALTOS_SRC = \ ao_fec_tx.c \ ao_fec_rx.c \ ao_seven_segment.c \ + ao_debounce.c \ ao_quadrature.c \ ao_button.c \ ao_event.c \ diff --git a/src/telelco-v0.2/ao_lco.c b/src/telelco-v0.2/ao_lco.c index 418c0509..6e490247 100644 --- a/src/telelco-v0.2/ao_lco.c +++ b/src/telelco-v0.2/ao_lco.c @@ -114,11 +114,9 @@ ao_lco_input(void) switch (event.unit) { case AO_QUADRATURE_PAD: if (!ao_lco_armed) { - if (event.value == ao_lco_pad) - break; - dir = ((int8_t) event.value - (int8_t) ao_lco_pad) > 0 ? 1 : -1; - new_pad = event.value; - while (!ao_lco_pad_present(new_pad)) { + dir = (int8_t) event.value; + new_pad = ao_lco_pad; + do { new_pad += dir; if (new_pad > AO_PAD_MAX_CHANNELS) new_pad = 0; @@ -126,21 +124,18 @@ ao_lco_input(void) new_pad = AO_PAD_MAX_CHANNELS - 1; if (new_pad == ao_lco_pad) break; - } + } while (!ao_lco_pad_present(new_pad)); if (new_pad != ao_lco_pad) { ao_lco_pad = new_pad; - ao_quadrature_count[AO_QUADRATURE_PAD] = ao_lco_pad; ao_lco_set_pad(); } } break; case AO_QUADRATURE_BOX: if (!ao_lco_armed) { - if (event.value == ao_lco_box) - break; - dir = ((int8_t) event.value - (int8_t) ao_lco_box) > 0 ? 1 : -1; - new_box = event.value; - while (!ao_lco_box_present(new_box)) { + dir = (int8_t) event.value; + new_box = ao_lco_box; + do { new_box += dir; if (new_box > ao_lco_max_box) new_box = ao_lco_min_box; @@ -148,8 +143,7 @@ ao_lco_input(void) new_box = ao_lco_min_box; if (new_box == ao_lco_box) break; - } - ao_quadrature_count[AO_QUADRATURE_PAD] = new_box; + } while (!ao_lco_box_present(new_box)); if (ao_lco_box != new_box) { ao_lco_box = new_box; ao_lco_got_channels = 0; -- cgit v1.2.3 From 988924b51980ad43e39bc4785a625ff25eb16449 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 9 Jun 2013 22:09:13 -0700 Subject: altos: Add fast-timer API. Use for quadrature and button drivers This splits the fast-timer portion out of the debounce helper code and shares that with the quadrature driver which now uses it directly. Signed-off-by: Keith Packard --- src/core/ao.h | 1 + src/core/ao_debounce.c | 163 ++++++++++++++++++++++++++++++++++++++ src/core/ao_debounce.h | 26 +++++- src/drivers/ao_button.c | 7 +- src/drivers/ao_quadrature.c | 130 ++++++++++++++---------------- src/stm/ao_debounce.c | 187 -------------------------------------------- src/stm/ao_fast_timer.c | 120 ++++++++++++++++++++++++++++ src/stm/ao_fast_timer.h | 34 ++++++++ src/stm/stm32l.h | 5 ++ src/telelco-v0.2/Makefile | 1 + src/telelco-v0.2/ao_pins.h | 1 - 11 files changed, 411 insertions(+), 264 deletions(-) create mode 100644 src/core/ao_debounce.c delete mode 100644 src/stm/ao_debounce.c create mode 100644 src/stm/ao_fast_timer.c create mode 100644 src/stm/ao_fast_timer.h (limited to 'src/core') diff --git a/src/core/ao.h b/src/core/ao.h index 0886260f..caa0ec19 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -75,6 +75,7 @@ typedef AO_PORT_TYPE ao_port_t; #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 */ diff --git a/src/core/ao_debounce.c b/src/core/ao_debounce.c new file mode 100644 index 00000000..b9d67729 --- /dev/null +++ b/src/core/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/core/ao_debounce.h b/src/core/ao_debounce.h index ebe290e1..19c620f5 100644 --- a/src/core/ao_debounce.h +++ b/src/core/ao_debounce.h @@ -22,13 +22,16 @@ struct ao_debounce { struct ao_debounce *next; /* time that pin value must be stable before accepting */ - int8_t hold; + 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 */ - int8_t count; + uint8_t count; /* This pin is running */ uint8_t running; @@ -40,6 +43,22 @@ struct ao_debounce { 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); @@ -49,4 +68,7 @@ _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/drivers/ao_button.c b/src/drivers/ao_button.c index 7b1fb530..25c0cd5c 100644 --- a/src/drivers/ao_button.c +++ b/src/drivers/ao_button.c @@ -74,9 +74,10 @@ _ao_button_set(struct ao_debounce *debounce, uint8_t value) static void ao_button_debounce_init(struct ao_debounce *debounce) { - debounce->hold = AO_BUTTON_DEBOUNCE_HOLD; - debounce->_get = _ao_button_get; - debounce->_set = _ao_button_set; + ao_debounce_config(debounce, + _ao_button_get, + _ao_button_set, + AO_BUTTON_DEBOUNCE_HOLD); } static void diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index cfa58da6..d07488d0 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -18,18 +18,12 @@ #include #include #include -#include +#include #include -#define AO_QUADRATURE_DEBOUNCE_HOLD 3 - -static __xdata struct ao_debounce ao_quadrature_debounce[AO_QUADRATURE_COUNT]; - -#define debounce_id(d) ((d) - ao_quadrature_debounce) - __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; - static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; +static int8_t ao_quadrature_raw[AO_QUADRATURE_COUNT]; #define BIT(a,b) ((a) | ((b) << 1)) #define STATE(old_a, old_b, new_a, new_b) (((BIT(old_a, old_b) << 2) | BIT(new_a, new_b))) @@ -39,32 +33,17 @@ static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; #define bitb(q) AO_QUADRATURE_ ## q ## _B #define pina(q) AO_QUADRATURE_ ## q ## _A ## _PIN #define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN +#define isr(q) ao_quadrature_isr_ ## q -#define q_case(q) case q: v = (!ao_gpio_get(port(q), bita(q), pina(q))) | ((!ao_gpio_get(port(q), bitb(q), pinb(q))) << 1); break - -uint8_t quad_raw[64]; -uint8_t quad_r; - -static uint8_t -_ao_quadrature_get(struct ao_debounce *debounce) { - uint8_t q = debounce_id(debounce); - uint8_t v = 0; +static inline uint16_t +ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b) { + uint16_t v = stm_gpio_get_all(gpio); - switch (q) { -#if AO_QUADRATURE_COUNT > 0 - q_case(0); -#endif -#if AO_QUADRATURE_COUNT > 1 - q_case(1); -#endif - } - if (q == 0) { - quad_raw[quad_r] = v; - quad_r = (quad_r + 1) & 63; - } - return v; + return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3; } +#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q)) + static void _ao_quadrature_queue(uint8_t q, int8_t step) { @@ -75,37 +54,51 @@ _ao_quadrature_queue(uint8_t q, int8_t step) ao_wakeup(&ao_quadrature_count[q]); } -uint8_t quad_history[64]; -uint8_t quad_h; +static const int8_t step[16] = { + [STATE(0,0,0,0)] = 0, + [STATE(0,0,0,1)] = -1, + [STATE(0,0,1,0)] = 1, + [STATE(0,0,1,1)] = 0, + [STATE(0,1,0,0)] = 1, + [STATE(0,1,1,0)] = 0, + [STATE(0,1,1,1)] = -1, + [STATE(1,0,0,0)] = -1, + [STATE(1,0,0,1)] = 0, + [STATE(1,0,1,0)] = 0, + [STATE(1,0,1,1)] = 1, + [STATE(1,1,0,0)] = 0, + [STATE(1,1,0,1)] = 1, + [STATE(1,1,1,0)] = -1, + [STATE(1,1,1,1)] = 0 +}; static void -_ao_quadrature_set(struct ao_debounce *debounce, uint8_t value) { - uint8_t q = debounce_id(debounce); +_ao_quadrature_set(uint8_t q, uint8_t value) { + uint8_t v; - ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); - ao_quadrature_state[q] |= value; - - if (q == 0) { - quad_history[quad_h] = ao_quadrature_state[0]; - quad_h = (quad_h + 1) & 63; - } + v = ao_quadrature_state[q] & 3; + value = value & 3; - switch (ao_quadrature_state[q]) { - case STATE(0, 1, 0, 0): - _ao_quadrature_queue(q, 1); - break; - case STATE(1, 0, 0, 0): - _ao_quadrature_queue(q, -1); - break; + if (v == value) + return; + + ao_quadrature_state[q] = (v << 2) | value; + + ao_quadrature_raw[q] += step[ao_quadrature_state[q]]; + if (value == 0) { + if (ao_quadrature_raw[q] == 4) + _ao_quadrature_queue(q, 1); + else if (ao_quadrature_raw[q] == -4) + _ao_quadrature_queue(q, -1); + ao_quadrature_raw[q] = 0; } } static void ao_quadrature_isr(void) { - uint8_t q; - for (q = 0; q < AO_QUADRATURE_COUNT; q++) - _ao_debounce_start(&ao_quadrature_debounce[q]); + _ao_quadrature_set(0, _ao_quadrature_get(0)); + _ao_quadrature_set(1, _ao_quadrature_get(1)); } int32_t @@ -130,6 +123,15 @@ ao_quadrature_test(void) ao_cmd_decimal(); q = ao_cmd_lex_i; + if (q >= AO_QUADRATURE_COUNT) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + printf ("count %d state %x raw %d\n", + ao_quadrature_count[q], + ao_quadrature_state[q], + ao_quadrature_raw[q]); +#if 0 for (;;) { int32_t c; flush(); @@ -138,6 +140,7 @@ ao_quadrature_test(void) if (c == 100) break; } +#endif } static const struct ao_cmds ao_quadrature_cmds[] = { @@ -145,36 +148,21 @@ static const struct ao_cmds ao_quadrature_cmds[] = { { 0, NULL } }; -static void -ao_quadrature_debounce_init(struct ao_debounce *debounce) { - debounce->hold = AO_QUADRATURE_DEBOUNCE_HOLD; - debounce->_get = _ao_quadrature_get; - debounce->_set = _ao_quadrature_set; -} - -#define init(q) do { \ - ao_enable_port(port(q)); \ - ao_quadrature_debounce_init(&ao_quadrature_debounce[q]); \ - ao_exti_setup(port(q), bita(q), \ - AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \ - ao_quadrature_isr); \ - ao_exti_enable(port(q), bita(q)); \ - \ - ao_exti_setup(port(q), bitb(q), \ - AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \ - ao_quadrature_isr); \ - ao_exti_enable(port(q), bitb(q)); \ +#define init(q) do { \ + ao_enable_input(port(q), bita(q), 0); \ + ao_enable_input(port(q), bitb(q), 0); \ } while (0) void ao_quadrature_init(void) { - ao_debounce_init(); #if AO_QUADRATURE_COUNT > 0 init(0); #endif #if AO_QUADRATURE_COUNT > 1 init(1); #endif + ao_fast_timer_init(); + ao_fast_timer_on(ao_quadrature_isr); ao_cmd_register(&ao_quadrature_cmds[0]); } diff --git a/src/stm/ao_debounce.c b/src/stm/ao_debounce.c deleted file mode 100644 index 22cdf230..00000000 --- a/src/stm/ao_debounce.c +++ /dev/null @@ -1,187 +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 - -static uint8_t ao_debounce_initialized; -static uint8_t ao_debounce_running; -static struct ao_debounce *ao_debounce; - -static void -ao_debounce_on(void) -{ - stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | - (0 << STM_TIM67_CR1_OPM) | - (1 << STM_TIM67_CR1_URS) | - (0 << STM_TIM67_CR1_UDIS) | - (1 << STM_TIM67_CR1_CEN)); -} - -static void -ao_debounce_off(void) -{ - stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | - (0 << STM_TIM67_CR1_OPM) | - (1 << STM_TIM67_CR1_URS) | - (0 << STM_TIM67_CR1_UDIS) | - (0 << STM_TIM67_CR1_CEN)); -} - -static void -_ao_debounce_set(struct ao_debounce *debounce, uint8_t value) -{ - if (value != debounce->value) { - debounce->value = value; - debounce->_set(debounce, value); - } - _ao_debounce_stop(debounce); -} - -/* - * Get the current value, set the result when we've - * reached the debounce count limit - */ -static void -_ao_debounce_check(struct ao_debounce *debounce) -{ - if (debounce->_get(debounce)) { - if (debounce->count < 0) - debounce->count = 0; - if (debounce->count < debounce->hold) { - if (++debounce->count == debounce->hold) - _ao_debounce_set(debounce, 1); - } - } else { - if (debounce->count > 0) - debounce->count = 0; - if (debounce->count > -debounce->hold) { - if (--debounce->count == -debounce->hold) - _ao_debounce_set(debounce, 0); - } - } -} - -/* - * Start monitoring one pin - */ -void -_ao_debounce_start(struct ao_debounce *debounce) -{ - if (debounce->running) - return; - 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); -} - -/* - * Stop monitoring one pin - */ -void -_ao_debounce_stop(struct ao_debounce *debounce) -{ - struct ao_debounce **prev; - if (!debounce->running) - return; - - 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(); -} - -void stm_tim6_isr(void) -{ - struct ao_debounce *debounce, *next; - if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) { - stm_tim6.sr = 0; - - /* Walk the current list, allowing the current - * object to be removed from the list - */ - for (debounce = ao_debounce; debounce; debounce = next) { - next = debounce->next; - _ao_debounce_check(debounce); - } - } -} - -/* - * According to the STM clock-configuration, timers run - * twice as fast as the APB1 clock *if* the APB1 prescaler - * is greater than 1. - */ - -#if AO_APB1_PRESCALER > 1 -#define TIMER_23467_SCALER 2 -#else -#define TIMER_23467_SCALER 1 -#endif - -#define TIMER_100kHz ((AO_PCLK1 * TIMER_23467_SCALER) / 100000) - -void -ao_debounce_init(void) -{ - if (ao_debounce_initialized) - return; - ao_debounce_initialized = 1; - - stm_nvic_set_enable(STM_ISR_TIM6_POS); - stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY); - - /* Turn on timer 6 */ - stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN); - - stm_tim6.psc = TIMER_100kHz; - stm_tim6.arr = 9; - stm_tim6.cnt = 0; - - /* Enable update interrupt */ - stm_tim6.dier = (1 << STM_TIM67_DIER_UIE); - - /* Poke timer to reload values */ - stm_tim6.egr |= (1 << STM_TIM67_EGR_UG); - - stm_tim6.cr2 = (STM_TIM67_CR2_MMS_RESET << STM_TIM67_CR2_MMS); - - /* And turn it off (for now) */ - ao_debounce_off(); -} diff --git a/src/stm/ao_fast_timer.c b/src/stm/ao_fast_timer.c new file mode 100644 index 00000000..d61b40c9 --- /dev/null +++ b/src/stm/ao_fast_timer.c @@ -0,0 +1,120 @@ +/* + * 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 + +static void (*ao_fast_timer_callback[AO_FAST_TIMER_MAX])(void); +static uint8_t ao_fast_timer_count; +static uint8_t ao_fast_timer_users; + +static void +ao_fast_timer_enable(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (1 << STM_TIM67_CR1_CEN)); +} + +static void +ao_fast_timer_disable(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (0 << STM_TIM67_CR1_CEN)); +} + +void +ao_fast_timer_on(void (*callback)(void)) +{ + ao_fast_timer_callback[ao_fast_timer_count] = callback; + if (!ao_fast_timer_count++) + ao_fast_timer_enable(); +} + +void +ao_fast_timer_off(void (*callback)(void)) +{ + uint8_t n; + + for (n = 0; n < ao_fast_timer_count; n++) + if (ao_fast_timer_callback[n] == callback) { + for (; n < ao_fast_timer_count-1; n++) { + ao_fast_timer_callback[n] = ao_fast_timer_callback[n+1]; + } + if (!--ao_fast_timer_count) + ao_fast_timer_disable(); + break; + } +} + +void stm_tim6_isr(void) +{ + uint8_t i; + if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) { + stm_tim6.sr = 0; + + for (i = 0; i < ao_fast_timer_count; i++) + (*ao_fast_timer_callback[i])(); + } +} + +/* + * According to the STM clock-configuration, timers run + * twice as fast as the APB1 clock *if* the APB1 prescaler + * is greater than 1. + */ + +#if AO_APB1_PRESCALER > 1 +#define TIMER_23467_SCALER 2 +#else +#define TIMER_23467_SCALER 1 +#endif + +#define TIMER_10kHz ((AO_PCLK1 * TIMER_23467_SCALER) / 10000) + +void +ao_fast_timer_init(void) +{ + if (!ao_fast_timer_users) { + stm_nvic_set_enable(STM_ISR_TIM6_POS); + stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY); + + /* Turn on timer 6 */ + stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN); + + stm_tim6.psc = TIMER_10kHz; + stm_tim6.arr = 9; + stm_tim6.cnt = 0; + + /* Enable update interrupt */ + stm_tim6.dier = (1 << STM_TIM67_DIER_UIE); + + /* Poke timer to reload values */ + stm_tim6.egr |= (1 << STM_TIM67_EGR_UG); + + stm_tim6.cr2 = (STM_TIM67_CR2_MMS_RESET << STM_TIM67_CR2_MMS); + ao_fast_timer_disable(); + } + if (ao_fast_timer_users == AO_FAST_TIMER_MAX) + ao_panic(AO_PANIC_FAST_TIMER); + ao_fast_timer_users++; +} + diff --git a/src/stm/ao_fast_timer.h b/src/stm/ao_fast_timer.h new file mode 100644 index 00000000..90fb3930 --- /dev/null +++ b/src/stm/ao_fast_timer.h @@ -0,0 +1,34 @@ +/* + * 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_FAST_TIMER_H_ +#define _AO_FAST_TIMER_H_ + +void +ao_fast_timer_init(void); + +#ifndef AO_FAST_TIMER_MAX +#define AO_FAST_TIMER_MAX 2 +#endif + +void +ao_fast_timer_on(void (*callback)(void)); + +void +ao_fast_timer_off(void (*callback)(void)); + +#endif /* _AO_FAST_TIMER_H_ */ diff --git a/src/stm/stm32l.h b/src/stm/stm32l.h index 63bde0f8..1868468f 100644 --- a/src/stm/stm32l.h +++ b/src/stm/stm32l.h @@ -171,6 +171,11 @@ stm_gpio_get(struct stm_gpio *gpio, int pin) { return (gpio->idr >> pin) & 1; } +static inline uint16_t +stm_gpio_get_all(struct stm_gpio *gpio) { + return gpio->idr; +} + extern struct stm_gpio stm_gpioa; extern struct stm_gpio stm_gpiob; extern struct stm_gpio stm_gpioc; diff --git a/src/telelco-v0.2/Makefile b/src/telelco-v0.2/Makefile index 42a5a7ee..f78ca9e2 100644 --- a/src/telelco-v0.2/Makefile +++ b/src/telelco-v0.2/Makefile @@ -50,6 +50,7 @@ ALTOS_SRC = \ ao_beep_stm.c \ ao_storage.c \ ao_eeprom_stm.c \ + ao_fast_timer.c \ ao_lcd_stm.c \ ao_usb_stm.c \ ao_exti_stm.c \ diff --git a/src/telelco-v0.2/ao_pins.h b/src/telelco-v0.2/ao_pins.h index 07ea1b45..d86782f3 100644 --- a/src/telelco-v0.2/ao_pins.h +++ b/src/telelco-v0.2/ao_pins.h @@ -235,7 +235,6 @@ */ #define AO_QUADRATURE_COUNT 2 -#define AO_QUADRATURE_MODE 0 #define AO_QUADRATURE_0_PORT &stm_gpioe #define AO_QUADRATURE_0_A 3 -- cgit v1.2.3 From b3ad488477def157e277e239e81f164b49725925 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 17 Jun 2013 13:58:41 -0700 Subject: altos: Disable USB on all flight computers when in flight mode There was a check to only disable USB on boards with radios, but for EasyMini, we want to disable USB too for flight mode. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 782e2274..1322195b 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -117,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 && !HAS_SAMPLE_PROFILE +#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE /* Disable the USB controller in flight mode * to save power */ -- cgit v1.2.3 From e148582217d6e02ac90a68e2bb2532947378d36f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 24 Jun 2013 14:28:06 -0700 Subject: altos: Support mega-style logging without ADC Used for TeleGPS, just exposes the necessary log writing function without also including the ADC writing code. Signed-off-by: Keith Packard --- src/core/ao_log.c | 5 +++++ src/core/ao_log_mega.c | 2 ++ 2 files changed, 7 insertions(+) (limited to 'src/core') diff --git a/src/core/ao_log.c b/src/core/ao_log.c index 7884ec3c..8bcb7707 100644 --- a/src/core/ao_log.c +++ b/src/core/ao_log.c @@ -278,6 +278,11 @@ ao_log_init(void) 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_mega.c b/src/core/ao_log_mega.c index ee12b907..768947d5 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -65,6 +65,7 @@ ao_log_dump_check_data(void) 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 */ @@ -182,6 +183,7 @@ ao_log(void) ao_sleep(&ao_log_running); } } +#endif uint16_t ao_log_flight(uint8_t slot) -- cgit v1.2.3 From e0a0a747624c2df66ca4a73b5a0de014ea204dca Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 17 Aug 2013 17:36:35 +0200 Subject: altos: allow projects to override default config values Override default radio power and APRS interval Signed-off-by: Keith Packard --- src/core/ao_config.c | 7 ++++++- src/core/ao_telemetry.c | 4 ++++ 2 files changed, 10 insertions(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_config.c b/src/core/ao_config.c index 1a500c79..b480e14c 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -28,6 +28,9 @@ __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" @@ -47,7 +50,9 @@ __xdata uint8_t ao_config_mutex; #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 @@ -142,7 +147,7 @@ _ao_config_get(void) memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro)); #endif if (minor < 13) - ao_config.aprs_interval = 0; + 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; diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index c3bbfec5..dfde2235 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -198,7 +198,11 @@ ao_send_configuration(void) { 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; -- cgit v1.2.3 From 4babe7310f78338ca36ab9d31ac833eada27485f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 24 Aug 2013 23:22:18 -0700 Subject: altos: Allow products to disable RDF entirely TeleGPS doesn't ever want RDF Signed-off-by: Keith Packard --- src/core/ao_telemetry.c | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index dfde2235..03a8a273 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -19,9 +19,16 @@ #include "ao_log.h" #include "ao_product.h" +#ifndef HAS_RDF +#define HAS_RDF 1 +#endif + static __pdata uint16_t ao_telemetry_interval; static __pdata uint8_t ao_rdf = 0; + +#if HAS_RDF static __pdata uint16_t ao_rdf_time; +#endif #if HAS_APRS static __pdata uint16_t ao_aprs_time; @@ -299,7 +306,10 @@ ao_telemetry(void) for (;;) { while (ao_telemetry_interval == 0) ao_sleep(&telemetry); - time = ao_rdf_time = ao_time(); + time = ao_time(); +#if HAS_RDF + ao_rdf_time = time; +#endif #if HAS_APRS ao_aprs_time = time; #endif @@ -332,6 +342,7 @@ ao_telemetry(void) #endif } #ifndef AO_SEND_ALL_BARO +#if HAS_RDF if (ao_rdf && #if HAS_APRS !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && @@ -349,6 +360,7 @@ ao_telemetry(void) #endif ao_radio_rdf(); } +#endif /* HAS_RDF */ #if HAS_APRS if (ao_config.aprs_interval != 0 && (int16_t) (ao_time() - ao_aprs_time) >= 0) @@ -415,6 +427,7 @@ ao_telemetry_set_interval(uint16_t interval) ao_wakeup(&telemetry); } +#if HAS_RDF void ao_rdf_set(uint8_t rdf) { @@ -425,6 +438,7 @@ ao_rdf_set(uint8_t rdf) ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; } } +#endif __xdata struct ao_task ao_telemetry_task; -- cgit v1.2.3 From d0b4e926ecececa7499a301b6135189be119512e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 22 May 2013 13:03:06 -0700 Subject: Initial TeleMini bits Signed-off-by: Keith Packard --- src/cc1111/ao_adc.c | 5 ++ src/cc1111/ao_usb.c | 5 ++ src/core/ao_data.c | 2 + src/core/ao_data.h | 2 +- src/core/ao_log_mini.c | 1 - src/core/ao_telemetry.c | 40 +++++++++- src/core/ao_telemetry.h | 27 +++++++ src/drivers/ao_ms5607.c | 40 +++++----- src/drivers/ao_ms5607.h | 2 +- src/drivers/ao_ms5607_convert_8051.c | 60 +++++++++++++++ src/telemini-v2.0/Makefile | 108 +++++++++++++++++++++++++++ src/telemini-v2.0/ao_pins.h | 140 +++++++++++++++++++++++++++++++++++ src/telemini-v2.0/ao_telemini.c | 54 ++++++++++++++ src/test/Makefile | 5 +- 14 files changed, 466 insertions(+), 25 deletions(-) create mode 100644 src/drivers/ao_ms5607_convert_8051.c create mode 100644 src/telemini-v2.0/Makefile create mode 100644 src/telemini-v2.0/ao_pins.h create mode 100644 src/telemini-v2.0/ao_telemini.c (limited to 'src/core') diff --git a/src/cc1111/ao_adc.c b/src/cc1111/ao_adc.c index 4a58023d..6cc08399 100644 --- a/src/cc1111/ao_adc.c +++ b/src/cc1111/ao_adc.c @@ -162,6 +162,11 @@ ao_adc_isr(void) __interrupt 1 #define GOT_ADC #endif +#ifdef FETCH_ADC + FETCH_ADC() +#define GOT_ADC +#endif + #ifndef GOT_ADC #error No known ADC configuration set #endif diff --git a/src/cc1111/ao_usb.c b/src/cc1111/ao_usb.c index a655d1be..b0ab409d 100644 --- a/src/cc1111/ao_usb.c +++ b/src/cc1111/ao_usb.c @@ -201,6 +201,11 @@ ao_usb_ep0_setup(void) ao_usb_ep0_queue_byte(0); break; case AO_USB_REQ_SET_ADDRESS: +#if USB_FORCE_FLIGHT_IDLE + /* Go to idle mode if USB is connected + */ + ao_flight_force_idle = 1; +#endif ao_usb_set_address(ao_usb_setup.value); break; case AO_USB_REQ_GET_DESCRIPTOR: diff --git a/src/core/ao_data.c b/src/core/ao_data.c index 38d2f7ff..6a3d02a1 100644 --- a/src/core/ao_data.c +++ b/src/core/ao_data.c @@ -22,6 +22,7 @@ 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) { @@ -32,3 +33,4 @@ ao_data_get(__xdata struct ao_data *packet) #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 index b0f086f8..080a534f 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -101,7 +101,7 @@ extern volatile __data uint8_t ao_data_count; * signaled by the timer tick */ #define AO_DATA_WAIT() do { \ - ao_sleep((void *) &ao_data_count); \ + ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count)); \ } while (0) #endif /* AO_DATA_RING */ diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c index 1273b0e3..46b285f3 100644 --- a/src/core/ao_log_mini.c +++ b/src/core/ao_log_mini.c @@ -79,7 +79,6 @@ void ao_log(void) { __pdata uint16_t next_sensor, next_other; - uint8_t i; ao_storage_setup(); diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 03a8a273..9c673030 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -171,6 +171,36 @@ ao_send_mega_data(void) } #endif /* AO_SEND_MEGA */ +#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 + #ifdef AO_SEND_ALL_BARO static uint8_t ao_baro_sample; @@ -323,12 +353,16 @@ ao_telemetry(void) ao_send_baro(); #endif #if HAS_FLIGHT -#ifdef AO_SEND_MEGA +# ifdef AO_SEND_MEGA ao_send_mega_sensor(); ao_send_mega_data(); -#else +# else +# ifdef AO_SEND_MINI + ao_send_mini(); +# else ao_send_sensor(); -#endif +# endif +# endif #endif #if HAS_COMPANION diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h index f2d201de..77601529 100644 --- a/src/core/ao_telemetry.h +++ b/src/core/ao_telemetry.h @@ -207,6 +207,32 @@ struct ao_telemetry_mega_data { }; +#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 @@ -240,6 +266,7 @@ union ao_telemetry_all { struct ao_telemetry_companion companion; struct ao_telemetry_mega_sensor mega_sensor; struct ao_telemetry_mega_data mega_data; + struct ao_telemetry_mini mini; struct ao_telemetry_baro baro; }; diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 8f1dcbe1..5259b265 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -21,7 +21,7 @@ #if HAS_MS5607 || HAS_MS5611 -static struct ao_ms5607_prom ms5607_prom; +static __xdata struct ao_ms5607_prom ms5607_prom; static uint8_t ms5607_configured; static void @@ -40,7 +40,7 @@ ao_ms5607_reset(void) { cmd = AO_MS5607_RESET; ao_ms5607_start(); - ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX); + ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX); ao_delay(AO_MS_TO_TICKS(10)); ao_ms5607_stop(); } @@ -69,17 +69,17 @@ ao_ms5607_crc(uint8_t *prom) } static void -ao_ms5607_prom_read(struct ao_ms5607_prom *prom) +ao_ms5607_prom_read(__xdata struct ao_ms5607_prom *prom) { - uint8_t addr; - uint8_t crc; - uint16_t *r; + uint8_t addr; + uint8_t crc; + __xdata uint16_t *r; - r = (uint16_t *) prom; + r = (__xdata uint16_t *) prom; for (addr = 0; addr < 8; addr++) { uint8_t cmd = AO_MS5607_PROM_READ(addr); ao_ms5607_start(); - ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX); + ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX); ao_spi_recv(r, 2, AO_MS5607_SPI_INDEX); ao_ms5607_stop(); r++; @@ -114,25 +114,25 @@ ao_ms5607_setup(void) ao_ms5607_prom_read(&ms5607_prom); } -static volatile uint8_t ao_ms5607_done; +static __xdata volatile uint8_t ao_ms5607_done; static void ao_ms5607_isr(void) { ao_exti_disable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN); ao_ms5607_done = 1; - ao_wakeup((void *) &ao_ms5607_done); + ao_wakeup((__xdata void *) &ao_ms5607_done); } static uint32_t ao_ms5607_get_sample(uint8_t cmd) { - uint8_t reply[3]; - uint8_t read; + __xdata uint8_t reply[3]; + __xdata uint8_t read; ao_ms5607_done = 0; ao_ms5607_start(); - ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX); + ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX); ao_exti_enable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN); @@ -179,10 +179,14 @@ ao_ms5607_sample(struct ao_ms5607_sample *sample) sample->temp = ao_ms5607_get_sample(AO_CONVERT_D2); } +#ifdef _CC1111_H_ +#include "ao_ms5607_convert_8051.c" +#else #include "ao_ms5607_convert.c" +#endif #if HAS_TASK -struct ao_ms5607_sample ao_ms5607_current; +__xdata struct ao_ms5607_sample ao_ms5607_current; static void ao_ms5607(void) @@ -191,10 +195,10 @@ ao_ms5607(void) for (;;) { ao_ms5607_sample(&ao_ms5607_current); - ao_arch_critical( - AO_DATA_PRESENT(AO_DATA_MS5607); - AO_DATA_WAIT(); - ); + ao_arch_block_interrupts(); + AO_DATA_PRESENT(AO_DATA_MS5607); + AO_DATA_WAIT(); + ao_arch_release_interrupts(); } } diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h index b2f98a59..3fd43fd4 100644 --- a/src/drivers/ao_ms5607.h +++ b/src/drivers/ao_ms5607.h @@ -56,7 +56,7 @@ struct ao_ms5607_value { int32_t temp; /* in °C * 100 */ }; -extern struct ao_ms5607_sample ao_ms5607_current; +extern __xdata struct ao_ms5607_sample ao_ms5607_current; void ao_ms5607_setup(void); diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c new file mode 100644 index 00000000..f47972c9 --- /dev/null +++ b/src/drivers/ao_ms5607_convert_8051.c @@ -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. + */ + +#include + +void +ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value) +{ +#if 0 + int32_t dT; + int32_t TEMP; + int64_t OFF; + int64_t SENS; + + dT = sample->temp - ((int32_t) ms5607_prom.tref << 8); + + TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); + +#if HAS_MS5611 + OFF = ((int64_t) ms5607_prom.off << 16) + (((int64_t) ms5607_prom.tco * dT) >> 7); + SENS = ((int64_t) ms5607_prom.sens << 15) + (((int64_t) ms5607_prom.tcs * dT) >> 8); +#else + OFF = ((int64_t) ms5607_prom.off << 17) + (((int64_t) ms5607_prom.tco * dT) >> 6); + SENS = ((int64_t) ms5607_prom.sens << 16) + (((int64_t) ms5607_prom.tcs * dT) >> 7); +#endif + + if (TEMP < 2000) { + int32_t T2 = ((int64_t) dT * (int64_t) dT) >> 31; + int32_t TEMPM = TEMP - 2000; + int64_t OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4; + int64_t SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM; + if (TEMP < 1500) { + int32_t TEMPP = TEMP + 1500; + int64_t TEMPP2 = TEMPP * TEMPP; + OFF2 = OFF2 + 15 * TEMPP2; + SENS2 = SENS2 + 8 * TEMPP2; + } + TEMP -= T2; + OFF -= OFF2; + SENS -= SENS2; + } + + value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; + value->temp = TEMP; +#endif +} diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile new file mode 100644 index 00000000..afd529f0 --- /dev/null +++ b/src/telemini-v2.0/Makefile @@ -0,0 +1,108 @@ +# +# TeleMini build file +# + +TELEMINI_VER=2.0 +TELEMINI_DEF=2_0 + +vpath %.c ..:../core:../cc1111:../drivers:../product +vpath %.h ..:../core:../cc1111:../drivers:../product +vpath ao-make-product.5c ../util + +ifndef VERSION +include ../Version +endif + +INC = \ + ao.h \ + ao_pins.h \ + ao_arch.h \ + ao_arch_funcs.h \ + cc1111.h \ + ao_product.h + +CORE_SRC = \ + ao_cmd.c \ + ao_config.c \ + ao_convert.c \ + ao_flight.c \ + ao_kalman.c \ + ao_log.c \ + ao_log_mini.c \ + ao_mutex.c \ + ao_panic.c \ + ao_report.c \ + ao_sample.c \ + ao_stdio.c \ + ao_storage.c \ + ao_task.c \ + ao_telemetry.c \ + ao_freq.c + +CC1111_SRC = \ + ao_adc.c \ + ao_dma.c \ + ao_ignite.c \ + ao_led.c \ + ao_packet.c \ + ao_packet_slave.c \ + ao_radio.c \ + ao_romconfig.c \ + ao_string.c \ + ao_spi.c \ + ao_usb.c \ + ao_convert_pa.c \ + ao_data.c \ + ao_beep.c \ + ao_timer.c \ + _bp.c + +DRIVER_SRC = \ + ao_ms5607.c \ + ao_m25.c + +PRODUCT_SRC = \ + ao_telemini.c + +SRC = \ + $(CORE_SRC) \ + $(CC1111_SRC) \ + $(DRIVER_SRC) \ + $(PRODUCT_SRC) + +PROGNAME = telemini-v$(TELEMINI_VER) +PROG = $(PROGNAME)-$(VERSION).ihx +PRODUCT=TeleMini-v$(TELEMINI_VER) +PRODUCT_DEF=-DTELEMINI_V_$(TELEMINI_DEF) +IDPRODUCT=0x000a + +include ../cc1111/Makefile.cc1111 + +NICKLE=nickle +CHECK_STACK=sh ../util/check-stack + +V=0 +# The user has explicitly enabled quiet compilation. +ifeq ($(V),0) +quiet = @printf " $1 $2 $@\n"; $($1) +endif +# Otherwise, print the full command line. +quiet ?= $($1) + +all: ../$(PROG) + +../$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. + $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +distclean: clean + +clean: clean-cc1111 + +install: + +uninstall: + diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h new file mode 100644 index 00000000..386c8dc3 --- /dev/null +++ b/src/telemini-v2.0/ao_pins.h @@ -0,0 +1,140 @@ +/* + * 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. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_RADIO 1 + +#define HAS_FLIGHT 1 +#define HAS_USB 1 +#define USB_FORCE_FLIGHT_IDLE 1 +#define HAS_BEEP 1 +#define HAS_GPS 0 +#define HAS_SERIAL_1 0 +#define HAS_EEPROM 1 +#define HAS_LOG 1 +#define USE_INTERNAL_FLASH 0 +#define HAS_DBG 0 +#define PACKET_HAS_SLAVE 1 +#define USE_FAST_ASCENT_LOG 1 + +#define AO_LED_GREEN 1 +#define AO_LED_RED 2 +#define LEDS_AVAILABLE (AO_LED_RED|AO_LED_GREEN) +#define HAS_EXTERNAL_TEMP 0 +#define HAS_ACCEL 0 +#define HAS_IGNITE 1 +#define HAS_IGNITE_REPORT 1 +#define HAS_MONITOR 0 + +/* + * SPI + */ + +#define SPI_CS_PORT P1 +#define SPI_CS_SEL P1SEL +#define SPI_CS_DIR P1DIR + +/* + * Flash + */ +#define AO_M25_SPI_CS_PORT SPI_CS_PORT +#define AO_M25_SPI_CS_MASK 0x04 /* cs_flash is P1_2 */ +#define M25_MAX_CHIPS 1 + +/* + * MS5607 + */ + +#define HAS_MS5607 1 +#define HAS_MS5611 0 +#define AO_MS5607_PRIVATE_PINS 0 +#define AO_MS5607_CS_PORT P1 +#define AO_MS5607_CS_PIN 3 +#define AO_MS5607_CS P1_3 +#define AO_MS5607_CS_MASK (1 << AO_MS5607_CS_PIN) +#define AO_MS5607_MISO_PORT P0 +#define AO_MS5607_MISO_PIN 2 +#define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO_PIN) +#define AO_MS5607_SPI_INDEX 0 + +/* + * Igniters + */ +#define AO_IGNITER_PORT P2 +#define AO_IGNITER_DROGUE_PORT AO_IGNITER_PORT +#define AO_IGNITER_DROGUE P2_3 +#define AO_IGNITER_MAIN P2_4 +#define AO_IGNITER_DIR P2DIR +#define AO_IGNITER_DROGUE_BIT (1 << 3) +#define AO_IGNITER_MAIN_BIT (1 << 4) +#define AO_IGNITER_DROGUE_PIN 3 +#define AO_IGNITER_MAIN_PIN 4 + +#define AO_IGNITER_DROGUE_PORT AO_IGNITER_PORT +#define AO_IGNITER_MAIN_PORT AO_IGNITER_PORT + +/* test these values with real igniters */ +#define AO_IGNITER_OPEN 1000 +#define AO_IGNITER_CLOSED 7000 +#define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) +#define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) + +#define AO_SEND_MINI + +/* + * ADC + */ + +#define HAS_ADC 1 +#define AO_ADC_FIRST_PIN 0 + +struct ao_adc { + int16_t sense_a; /* apogee continuity sense */ + int16_t sense_m; /* main continuity sense */ + int16_t v_batt; /* battery voltage */ +}; + +#define ao_data_count ao_adc_count + +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) + +#define AO_ADC_DUMP(p) \ + printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \ + (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt) + +#define FETCH_ADC() \ + a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc); \ + switch (sequence) { \ + case 4: \ + a += 4; \ + sequence = 0; \ + break; \ + case 1: \ + a += 2; \ + sequence = 4; \ + break; \ + case 0: \ + sequence = 1; \ + break; \ + } \ + if (sequence) \ + ; + +#endif /* _AO_PINS_H_ */ diff --git a/src/telemini-v2.0/ao_telemini.c b/src/telemini-v2.0/ao_telemini.c new file mode 100644 index 00000000..84df2b8c --- /dev/null +++ b/src/telemini-v2.0/ao_telemini.c @@ -0,0 +1,54 @@ +/* + * 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_pins.h" + +__xdata uint8_t ao_force_freq; + +void +main(void) +{ + /* + * Reduce the transient on the ignite pins at startup by + * pulling the pins low as soon as possible at power up + */ + ao_ignite_set_pins(); + + ao_clock_init(); + + /* Turn on the red LED until the system is stable */ + ao_led_init(LEDS_AVAILABLE); + ao_led_on(AO_LED_RED); + + ao_task_init(); + + ao_timer_init(); + ao_adc_init(); + ao_cmd_init(); + ao_storage_init(); + ao_ms5607_init(); + ao_flight_init(); + ao_log_init(); + ao_report_init(); + ao_telemetry_init(); + ao_radio_init(); + ao_packet_slave_init(TRUE); + ao_igniter_init(); + ao_config_init(); + ao_start_scheduler(); +} diff --git a/src/test/Makefile b/src/test/Makefile index 75b1f848..9c304318 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -2,7 +2,7 @@ vpath % ..:../core:../drivers:../util:../micropeak:../aes 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 \ - ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test + ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -73,3 +73,6 @@ ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c cc $(CFLAGS) -o $@ ao_aes_test.c + +ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h + cc $(CFLAGS) -o $@ ao_int64_test.c -- cgit v1.2.3 From 3114baef45803250a2e5cdd2ee4a9171f2045b0c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 22 May 2013 14:32:50 -0700 Subject: altos: Add 64-bit add/mul/shift for SDCC SDCC doeesn't provide a native 64-bit type (sigh), so implement the minimal operations necessary for the MS5607 conversion routine. Signed-off-by: Keith Packard --- src/core/ao_int64.c | 119 +++++++++++++++++++++++++++++++++++++++++++++++ src/core/ao_int64.h | 39 ++++++++++++++++ src/test/ao_int64_test.c | 78 +++++++++++++++++++++++++++++++ 3 files changed, 236 insertions(+) create mode 100644 src/core/ao_int64.c create mode 100644 src/core/ao_int64.h create mode 100644 src/test/ao_int64_test.c (limited to 'src/core') diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c new file mode 100644 index 00000000..1fe717dc --- /dev/null +++ b/src/core/ao_int64.c @@ -0,0 +1,119 @@ +/* + * 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 + +void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { + uint32_t t; + + r->low = t = a->low + b->low; + r->high = a->high + b->high; + if (t < a->low) + r->high++; +} + +void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { + if (d < 32) { + r->high = (int32_t) a->high >> d; + r->low = a->low >> d; + if (d) + r->low |= a->high << (32 - d); + } else { + d &= 0x1f; + r->high = 0; + r->low = (int32_t) a->high >> d; + } +} + +void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { + if (d < 32) { + r->high = a->high << d; + r->low = a->low << d; + if (d) + r->high |= a->low >> (32 - d); + } else { + d &= 0x1f; + r->low = 0; + r->high = a->low << d; + } +} + +void ao_umul64(ao_int64_t *r, uint32_t a, uint32_t b) +{ + uint32_t r1; + uint32_t r2, r3, r4; + ao_int64_t s,t,u,v; + r1 = (uint32_t) (uint16_t) a * (uint16_t) b; + r2 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b; + r3 = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16); + r4 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16); + + s.low = r1; + s.high = r4; + + t.high = (uint32_t) r2 >> 16; + t.low = r2 << 16; + ao_plus64(&u, &s, &t); + + v.high = (int32_t) r3 >> 16; + v.low = r3 << 16; + ao_plus64(r, &u, &v); +} + +void ao_neg64(ao_int64_t *r, ao_int64_t *a) { + r->high = ~a->high; + r->low = ~a->low; + if (!++r->low) + r->high++; +} + +void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) { + uint8_t negative = 0; + + if (a < 0) { + a = -a; + negative = 1; + } + if (b < 0) { + b = -b; + negative = !negative; + } + ao_umul64(r, a, b); + if (negative) + ao_neg64(r, r); +} + +void ao_umul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { + uint32_t low = a->low; + ao_umul64(r, (uint32_t) low >> 1, (uint32_t) b << 1); + if (low & 1) { + if ((uint32_t) (r->low += b) < (uint32_t) b) + r->high++; + } + r->high += a->high * b; +} + +void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { + if ((int32_t) a->high < 0) { + ao_int64_t t; + + ao_neg64(&t, a); + ao_umul64_16(r, &t, b); + ao_neg64(r, r); + } else + ao_umul64_16(r, a, b); +} diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h new file mode 100644 index 00000000..93aa87e4 --- /dev/null +++ b/src/core/ao_int64.h @@ -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_INT64_H_ +#define _AO_INT64_H_ + +#include + +typedef struct { + uint32_t high; + uint32_t low; +} ao_int64_t; + +void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); +void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); +void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); +void ao_mul64(ao_int64_t *r, int32_t a, int32_t b); +void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b); + +#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) + +#endif /* _AO_INT64_H_ */ diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c new file mode 100644 index 00000000..67ba6ec5 --- /dev/null +++ b/src/test/ao_int64_test.c @@ -0,0 +1,78 @@ +/* + * 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 +#include + +int errors; + +#define test(op,func,a,b,ao_a,ao_b) do { \ + r = (a) op (b); \ + func(&ao_r, ao_a, ao_b); \ + c = ao_cast64(&ao_r); \ + if (c != r) { \ + printf ("trial %4d: %lld " #func " %lld = %lld (should be %lld)\n", \ + trial, (int64_t) (a), (int64_t) b, c, r); \ + ++errors; \ + } \ + } while (0) + + +void +do_test(int trial, int64_t a, int64_t b) +{ + int64_t r, c; + ao_int64_t ao_a, ao_b, ao_r; + + ao_int64_init64(&ao_a, a >> 32, a); + ao_int64_init64(&ao_b, b >> 32, b); + + test(+, ao_plus64, a, b, &ao_a, &ao_b); + test(*, ao_mul64,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b); + test(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); + test(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); + test(*, ao_mul64_16, a, (uint16_t) b, &ao_a, (uint16_t) b); +} + +#define TESTS 10000000 + +static int64_t +random64(void) +{ + return (int64_t) random() + ((int64_t) random() << 31) /* + ((int64_t) random() << 33) */; +} + +int +main (int argc, char **argv) +{ + int i, start; + + if (argv[1]) + start = atoi(argv[1]); + else + start = 0; + srandom(1000); + for (i = 0; i < TESTS; i++) { + int64_t a = random64(); + int64_t b = random64(); + if (i >= start) + do_test(i, a, b); + } + return errors; +} -- cgit v1.2.3 From f7602ae566a5cbf2d2cbb1d68bad7e2d1177a33a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 22 May 2013 14:38:19 -0700 Subject: altos: Make 64x16 mul a bit faster the unsigned 32x32 multiply really does work, just use it Signed-off-by: Keith Packard --- src/core/ao_int64.c | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c index 1fe717dc..8e3caa24 100644 --- a/src/core/ao_int64.c +++ b/src/core/ao_int64.c @@ -86,11 +86,11 @@ void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) { if (a < 0) { a = -a; - negative = 1; + negative = ~0; } if (b < 0) { b = -b; - negative = !negative; + negative = ~negative; } ao_umul64(r, a, b); if (negative) @@ -98,12 +98,7 @@ void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) { } void ao_umul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { - uint32_t low = a->low; - ao_umul64(r, (uint32_t) low >> 1, (uint32_t) b << 1); - if (low & 1) { - if ((uint32_t) (r->low += b) < (uint32_t) b) - r->high++; - } + ao_umul64(r, a->low, b); r->high += a->high * b; } -- cgit v1.2.3 From 5ccd902d0fd2adc40c72982babb60fac4da6a087 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 22 May 2013 17:08:55 -0700 Subject: altos: Add 64x64 multiply. Test 64 ops for dest same as either source The test change is to ensure that the destination may be one of the 64 bit sources. Signed-off-by: Keith Packard --- src/core/ao_int64.c | 71 +++++++++++++++++++++++++++++++++++------------- src/core/ao_int64.h | 9 ++++-- src/test/ao_int64_test.c | 36 +++++++++++++++++++++--- 3 files changed, 91 insertions(+), 25 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c index 8e3caa24..5307342d 100644 --- a/src/core/ao_int64.c +++ b/src/core/ao_int64.c @@ -20,39 +20,40 @@ void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { uint32_t t; - r->low = t = a->low + b->low; r->high = a->high + b->high; + t = a->low + b->low; if (t < a->low) r->high++; + r->low = t; } void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { if (d < 32) { - r->high = (int32_t) a->high >> d; r->low = a->low >> d; if (d) r->low |= a->high << (32 - d); + r->high = (int32_t) a->high >> d; } else { d &= 0x1f; - r->high = 0; r->low = (int32_t) a->high >> d; + r->high = 0; } } void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { if (d < 32) { r->high = a->high << d; - r->low = a->low << d; if (d) r->high |= a->low >> (32 - d); + r->low = a->low << d; } else { d &= 0x1f; - r->low = 0; r->high = a->low << d; + r->low = 0; } } -void ao_umul64(ao_int64_t *r, uint32_t a, uint32_t b) +static void ao_umul64_32_32(ao_int64_t *r, uint32_t a, uint32_t b) { uint32_t r1; uint32_t r2, r3, r4; @@ -65,11 +66,11 @@ void ao_umul64(ao_int64_t *r, uint32_t a, uint32_t b) s.low = r1; s.high = r4; - t.high = (uint32_t) r2 >> 16; + t.high = r2 >> 16; t.low = r2 << 16; ao_plus64(&u, &s, &t); - v.high = (int32_t) r3 >> 16; + v.high = r3 >> 16; v.low = r3 << 16; ao_plus64(r, &u, &v); } @@ -81,7 +82,7 @@ void ao_neg64(ao_int64_t *r, ao_int64_t *a) { r->high++; } -void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) { +void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b) { uint8_t negative = 0; if (a < 0) { @@ -92,23 +93,55 @@ void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) { b = -b; negative = ~negative; } - ao_umul64(r, a, b); + ao_umul64_32_32(r, a, b); if (negative) ao_neg64(r, r); } -void ao_umul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { - ao_umul64(r, a->low, b); - r->high += a->high * b; +static void ao_umul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { + 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; } -void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { - if ((int32_t) a->high < 0) { - ao_int64_t t; +void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { + uint8_t negative = 0; + ao_int64_t ap, bp; - ao_neg64(&t, a); - ao_umul64_16(r, &t, b); + if (ao_int64_negativep(a)) { + ao_neg64(&ap, a); + a = ≈ + negative = ~0; + } + if (ao_int64_negativep(b)) { + ao_neg64(&bp, b); + b = &bp; + negative = ~negative; + } + ao_umul64(r, a, b); + if (negative) ao_neg64(r, r); +} + +void ao_umul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { + uint32_t h = a->high * b; + ao_umul64_32_32(r, a->low, b); + r->high += h; +} + +void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { + ao_int64_t ap; + uint8_t negative = 0; + if ((int32_t) a->high < 0) { + ao_neg64(&ap, a); + a = ≈ + negative = ~0; } else - ao_umul64_16(r, a, b); + 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 index 93aa87e4..e5eee823 100644 --- a/src/core/ao_int64.h +++ b/src/core/ao_int64.h @@ -26,14 +26,19 @@ typedef struct { } ao_int64_t; void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); +void ao_neg64(ao_int64_t *r, ao_int64_t *a); +void ao_lshift64_16(ao_int64_t *r, uint16_t a, uint8_t d); void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); -void ao_mul64(ao_int64_t *r, int32_t a, int32_t b); -void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b); +void ao_mul64_64_64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); +void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b); +void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b); #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/test/ao_int64_test.c b/src/test/ao_int64_test.c index 67ba6ec5..4c88b1a1 100644 --- a/src/test/ao_int64_test.c +++ b/src/test/ao_int64_test.c @@ -22,17 +22,35 @@ int errors; -#define test(op,func,a,b,ao_a,ao_b) do { \ +#define test_o(op,func,mod,a,b,ao_a,ao_b) do { \ r = (a) op (b); \ func(&ao_r, ao_a, ao_b); \ c = ao_cast64(&ao_r); \ if (c != r) { \ - printf ("trial %4d: %lld " #func " %lld = %lld (should be %lld)\n", \ + printf ("trial %4d: %lld " #func mod " %lld = %lld (should be %lld)\n", \ trial, (int64_t) (a), (int64_t) b, c, r); \ ++errors; \ } \ } while (0) +#define test(op,func,a,b,ao_a,ao_b) test_o(op,func,"",a,b,ao_a,ao_b) + +#define test_a(op,func,a,b,ao_a,ao_b) do { \ + ao_r = *ao_a; \ + test_o(op,func,"_a",a,b,&ao_r,ao_b); \ + } while (0) + +#define test_b(op,func,a,b,ao_a,ao_b) do { \ + ao_r = *ao_b; \ + test_o(op,func,"_b",a,b,ao_a,&ao_r); \ + } while (0) + +#define test_x(op,func,a,b,ao_a,ao_b) do { \ + ao_r = *ao_a; \ + test_o(op,func,"_xa",a,a,&ao_r,&ao_r); \ + ao_r = *ao_b; \ + test_o(op,func,"_xb",b,b,&ao_r,&ao_r); \ + } while (0) void do_test(int trial, int64_t a, int64_t b) @@ -44,10 +62,20 @@ do_test(int trial, int64_t a, int64_t b) ao_int64_init64(&ao_b, b >> 32, b); test(+, ao_plus64, a, b, &ao_a, &ao_b); - test(*, ao_mul64,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b); + test_a(+, ao_plus64, a, b, &ao_a, &ao_b); + test_b(+, ao_plus64, a, b, &ao_a, &ao_b); + test_x(+, ao_plus64, a, b, &ao_a, &ao_b); + test(*, ao_mul64_32_32,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b); + test(*, ao_mul64, a, b, &ao_a, &ao_b); + test_a(*, ao_mul64, a, b, &ao_a, &ao_b); + test_b(*, ao_mul64, a, b, &ao_a, &ao_b); + test_x(*, ao_mul64, a, b, &ao_a, &ao_b); + test(*, ao_mul64_64_16, a, (uint16_t) b, &ao_a, (uint16_t) b); + test_a(*, ao_mul64_64_16, a, (uint16_t) b, &ao_a, (uint16_t) b); test(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); + test_a(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); test(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); - test(*, ao_mul64_16, a, (uint16_t) b, &ao_a, (uint16_t) b); + test_a(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); } #define TESTS 10000000 -- cgit v1.2.3 From cb844328322fd7d9f4dafb58b322257a70b347e6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 22 May 2013 19:20:54 -0600 Subject: altos: Add 64-bit subtraction Signed-off-by: Keith Packard --- src/core/ao_int64.c | 11 +++++++++++ src/core/ao_int64.h | 2 ++ src/test/ao_int64_test.c | 4 ++++ 3 files changed, 17 insertions(+) (limited to 'src/core') diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c index 5307342d..07cdb357 100644 --- a/src/core/ao_int64.c +++ b/src/core/ao_int64.c @@ -27,6 +27,17 @@ void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { r->low = t; } +void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { + 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(ao_int64_t *r, ao_int64_t *a, uint8_t d) { if (d < 32) { r->low = a->low >> d; diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h index e5eee823..cf12f0d8 100644 --- a/src/core/ao_int64.h +++ b/src/core/ao_int64.h @@ -26,6 +26,7 @@ typedef struct { } ao_int64_t; void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); +void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); void ao_neg64(ao_int64_t *r, ao_int64_t *a); void ao_lshift64_16(ao_int64_t *r, uint16_t a, uint8_t d); void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); @@ -33,6 +34,7 @@ void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); void ao_mul64_64_64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b); void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b); +void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); #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)) diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c index 4c88b1a1..c26a63b0 100644 --- a/src/test/ao_int64_test.c +++ b/src/test/ao_int64_test.c @@ -65,6 +65,10 @@ do_test(int trial, int64_t a, int64_t b) test_a(+, ao_plus64, a, b, &ao_a, &ao_b); test_b(+, ao_plus64, a, b, &ao_a, &ao_b); test_x(+, ao_plus64, a, b, &ao_a, &ao_b); + test(-, ao_minus64, a, b, &ao_a, &ao_b); + test_a(-, ao_minus64, a, b, &ao_a, &ao_b); + test_b(-, ao_minus64, a, b, &ao_a, &ao_b); + test_x(-, ao_minus64, a, b, &ao_a, &ao_b); test(*, ao_mul64_32_32,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b); test(*, ao_mul64, a, b, &ao_a, &ao_b); test_a(*, ao_mul64, a, b, &ao_a, &ao_b); -- cgit v1.2.3 From 56911f27376b0fe91a464e369bb8aa1531b3c7dc Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 23 May 2013 02:17:51 -0600 Subject: altos: Make TeleMini v2.0 fit Mash lots of storage locations and code around to shrink stuff down to size Signed-off-by: Keith Packard --- src/core/ao.h | 2 +- src/core/ao_cmd.c | 4 +- src/core/ao_int64.c | 88 ++++++++++++------------ src/core/ao_int64.h | 22 +++--- src/core/ao_kalman.c | 4 +- src/core/ao_log_telem.c | 2 +- src/core/ao_sample.h | 4 +- src/core/ao_task.h | 3 + src/drivers/ao_ms5607.c | 4 +- src/drivers/ao_ms5607.h | 7 +- src/drivers/ao_ms5607_convert_8051.c | 126 ++++++++++++++++++++++++++++------- src/telemini-v2.0/Makefile | 9 ++- src/telemini-v2.0/ao_pins.h | 2 + src/test/Makefile | 6 +- src/test/ao_int64_test.c | 5 ++ src/test/ao_ms5607_convert_test.c | 96 ++++++++++++++++++++++++++ 16 files changed, 289 insertions(+), 95 deletions(-) create mode 100644 src/test/ao_ms5607_convert_test.c (limited to 'src/core') diff --git a/src/core/ao.h b/src/core/ao.h index caa0ec19..e7320327 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -182,7 +182,7 @@ void ao_cmd_hex(void); void -ao_cmd_decimal(void); +ao_cmd_decimal(void) __reentrant; /* Read a single hex nibble off stdin. */ uint8_t diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c index 5113548b..4ebaa607 100644 --- a/src/core/ao_cmd.c +++ b/src/core/ao_cmd.c @@ -206,9 +206,9 @@ ao_cmd_hex(void) } void -ao_cmd_decimal(void) +ao_cmd_decimal(void) __reentrant { - __pdata uint8_t r = ao_cmd_lex_error; + uint8_t r = ao_cmd_lex_error; ao_cmd_lex_u32 = 0; ao_cmd_white(); diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c index 07cdb357..aa23dbe0 100644 --- a/src/core/ao_int64.c +++ b/src/core/ao_int64.c @@ -17,8 +17,10 @@ #include -void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { - uint32_t t; +__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; @@ -27,9 +29,8 @@ void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { r->low = t; } -void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { - uint32_t 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; @@ -38,7 +39,7 @@ void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { r->low = t; } -void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { +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) @@ -51,7 +52,7 @@ void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { } } -void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { +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) @@ -64,53 +65,49 @@ void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { } } -static void ao_umul64_32_32(ao_int64_t *r, uint32_t a, uint32_t b) -{ - uint32_t r1; - uint32_t r2, r3, r4; - ao_int64_t s,t,u,v; - r1 = (uint32_t) (uint16_t) a * (uint16_t) b; - r2 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b; - r3 = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16); - r4 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16); - - s.low = r1; - s.high = r4; - - t.high = r2 >> 16; - t.low = r2 << 16; - ao_plus64(&u, &s, &t); - - v.high = r3 >> 16; - v.low = r3 << 16; - ao_plus64(r, &u, &v); +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(ao_int64_t *r, ao_int64_t *a) { +void ao_neg64(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR { r->high = ~a->high; - r->low = ~a->low; - if (!++r->low) + if (!(r->low = ~a->low + 1)) r->high++; } -void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b) { +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 = ~0; + ++negative; } if (b < 0) { b = -b; - negative = ~negative; + --negative; } ao_umul64_32_32(r, a, b); if (negative) ao_neg64(r, r); } -static void ao_umul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { - ao_int64_t r2, r3; +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); @@ -119,38 +116,41 @@ static void ao_umul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { r->high += r2.low + r3.low; } -void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { +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; - ao_int64_t ap, bp; if (ao_int64_negativep(a)) { ao_neg64(&ap, a); a = ≈ - negative = ~0; + ++negative; } if (ao_int64_negativep(b)) { ao_neg64(&bp, b); b = &bp; - negative = ~negative; + --negative; } ao_umul64(r, a, b); if (negative) ao_neg64(r, r); } -void ao_umul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { - uint32_t h = a->high * b; +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(ao_int64_t *r, ao_int64_t *a, uint16_t b) { - ao_int64_t ap; +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 = ~0; + negative++; } else ao_umul64_64_16(r, a, b); if (negative) diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h index cf12f0d8..b16db58c 100644 --- a/src/core/ao_int64.h +++ b/src/core/ao_int64.h @@ -25,16 +25,18 @@ typedef struct { uint32_t low; } ao_int64_t; -void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); -void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); -void ao_neg64(ao_int64_t *r, ao_int64_t *a); -void ao_lshift64_16(ao_int64_t *r, uint16_t a, uint8_t d); -void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); -void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); -void ao_mul64_64_64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); -void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b); -void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b); -void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); +#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)) diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c index 59ffd8b2..762b2c0a 100644 --- a/src/core/ao_kalman.c +++ b/src/core/ao_kalman.c @@ -40,9 +40,9 @@ static __pdata int32_t ao_k_accel; __pdata int16_t ao_height; __pdata int16_t ao_speed; __pdata int16_t ao_accel; -__pdata int16_t ao_max_height; +__xdata int16_t ao_max_height; static __pdata int32_t ao_avg_height_scaled; -__pdata int16_t ao_avg_height; +__xdata int16_t ao_avg_height; __pdata int16_t ao_error_h; __pdata int16_t ao_error_h_sq_avg; diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c index 23ebf7dd..095aca37 100644 --- a/src/core/ao_log_telem.c +++ b/src/core/ao_log_telem.c @@ -23,7 +23,7 @@ __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; -__pdata int16_t ao_max_height; /* max of ao_height */ +__xdata int16_t ao_max_height; /* max of ao_height */ __pdata int16_t sense_d, sense_m; __pdata uint8_t ao_igniter_present; diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index a2dac979..5bd29536 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -136,8 +136,8 @@ uint8_t ao_sample(void); 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 __pdata int16_t ao_max_height; /* max of ao_height */ -extern __pdata int16_t ao_avg_height; /* running average of height */ +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; diff --git a/src/core/ao_task.h b/src/core/ao_task.h index 1a4b5b6b..e3a311ed 100644 --- a/src/core/ao_task.h +++ b/src/core/ao_task.h @@ -45,7 +45,10 @@ struct ao_task { #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]; diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 5259b265..4b4403a7 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -173,7 +173,7 @@ ao_ms5607_get_sample(uint8_t cmd) { #define AO_CONVERT_D2 token_evaluator(AO_MS5607_CONVERT_D2_, AO_MS5607_TEMP_OVERSAMPLE) void -ao_ms5607_sample(struct ao_ms5607_sample *sample) +ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample) { sample->pres = ao_ms5607_get_sample(AO_CONVERT_D1); sample->temp = ao_ms5607_get_sample(AO_CONVERT_D2); @@ -220,7 +220,7 @@ ao_ms5607_info(void) static void ao_ms5607_dump(void) { - struct ao_ms5607_value value; + __xdata struct ao_ms5607_value value; ao_ms5607_convert(&ao_ms5607_current, &value); printf ("Pressure: %8u %8d\n", ao_ms5607_current.pres, value.pres); diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h index 3fd43fd4..206efd64 100644 --- a/src/drivers/ao_ms5607.h +++ b/src/drivers/ao_ms5607.h @@ -68,12 +68,13 @@ void ao_ms5607_info(void); void -ao_ms5607_sample(struct ao_ms5607_sample *sample); +ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample); void -ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value); +ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample, + __xdata struct ao_ms5607_value *value); void -ao_ms5607_get_prom(struct ao_ms5607_prom *prom); +ao_ms5607_get_prom(__data struct ao_ms5607_prom *prom); #endif /* _AO_MS5607_H_ */ diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c index f47972c9..f3a48c46 100644 --- a/src/drivers/ao_ms5607_convert_8051.c +++ b/src/drivers/ao_ms5607_convert_8051.c @@ -16,45 +16,121 @@ */ #include +#include + +#if HAS_MS5611 +#define SHIFT_OFF 16 +#define SHIFT_TCO 7 +#define SHIFT_SENS 15 +#define SHFIT_TCS 8 +#else +#define SHIFT_OFF 17 +#define SHIFT_TCO 6 +#define SHIFT_SENS 16 +#define SHIFT_TCS 7 +#endif void -ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value) +ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample, + __xdata struct ao_ms5607_value *value) { -#if 0 - int32_t dT; - int32_t TEMP; - int64_t OFF; - int64_t SENS; + __LOCAL int32_t dT; + __LOCAL int32_t TEMP; + __LOCAL ao_int64_t OFF; + __LOCAL ao_int64_t SENS; + __LOCAL ao_int64_t a; dT = sample->temp - ((int32_t) ms5607_prom.tref << 8); - TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); + /* TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); */ + ao_mul64_32_32(&a, dT, ms5607_prom.tempsens); + ao_rshift64(&a, &a, 23); + TEMP = 2000 + a.low; + /* */ -#if HAS_MS5611 - OFF = ((int64_t) ms5607_prom.off << 16) + (((int64_t) ms5607_prom.tco * dT) >> 7); - SENS = ((int64_t) ms5607_prom.sens << 15) + (((int64_t) ms5607_prom.tcs * dT) >> 8); + /* OFF = ((int64_t) ms5607_prom.off << SHIFT_OFF) + (((int64_t) ms5607_prom.tco * dT) >> SHIFT_TCO);*/ +#if SHIFT_OFF > 16 + OFF.high = ms5607_prom.off >> (32 - SHIFT_OFF); #else - OFF = ((int64_t) ms5607_prom.off << 17) + (((int64_t) ms5607_prom.tco * dT) >> 6); - SENS = ((int64_t) ms5607_prom.sens << 16) + (((int64_t) ms5607_prom.tcs * dT) >> 7); + OFF.high = 0; #endif + OFF.low = (uint32_t) ms5607_prom.off << SHIFT_OFF; + ao_mul64_32_32(&a, ms5607_prom.tco, dT); + ao_rshift64(&a, &a, SHIFT_TCO); + ao_plus64(&OFF, &OFF, &a); + /**/ + + /* SENS = ((int64_t) ms5607_prom.sens << SHIFT_SENS) + (((int64_t) ms5607_prom.tcs * dT) >> SHIFT_TCS); */ + SENS.high = 0; + SENS.low = (uint32_t) ms5607_prom.sens << SHIFT_SENS; + ao_mul64_32_32(&a, ms5607_prom.tcs, dT); + ao_rshift64(&a, &a, SHIFT_TCS); + ao_plus64(&SENS, &SENS, &a); + /**/ if (TEMP < 2000) { - int32_t T2 = ((int64_t) dT * (int64_t) dT) >> 31; - int32_t TEMPM = TEMP - 2000; - int64_t OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4; - int64_t SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM; - if (TEMP < 1500) { - int32_t TEMPP = TEMP + 1500; - int64_t TEMPP2 = TEMPP * TEMPP; - OFF2 = OFF2 + 15 * TEMPP2; - SENS2 = SENS2 + 8 * TEMPP2; + __LOCAL int32_t T2; + __LOCAL int32_t TEMPM; + __LOCAL ao_int64_t OFF2; + __LOCAL ao_int64_t SENS2; + + /* T2 = ((int64_t) dT * (int64_t) dT) >> 31; */ + ao_mul64_32_32(&a, dT, dT); + T2 = (a.low >> 31) | (a.high << 1); + /**/ + + TEMPM = TEMP - 2000; + + /* OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4; */ + ao_mul64_32_32(&OFF2, TEMPM, TEMPM); + ao_mul64_64_16(&OFF2, &OFF2, 61); + ao_rshift64(&OFF2, &OFF2, 4); + /**/ + + /* SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM; */ + ao_mul64_32_32(&SENS2, TEMPM, TEMPM); + ao_lshift64(&SENS2, &SENS2, 1); + /**/ + + if (TEMP < -1500) { + int32_t TEMPP; + int32_t TEMPP2; + + TEMPP = TEMP + 1500; + TEMPP2 = TEMPP * TEMPP; + + /* OFF2 = OFF2 + 15 * TEMPP2; */ + ao_mul64_32_32(&a, 15, TEMPP2); + ao_plus64(&OFF2, &OFF2, &a); + /**/ + + /* SENS2 = SENS2 + 8 * TEMPP2; */ + a.high = 0; + a.low = TEMPP2; + ao_lshift64(&a, &a, 3); + ao_plus64(&SENS2, &SENS2, &a); + /**/ } TEMP -= T2; - OFF -= OFF2; - SENS -= SENS2; + + /* OFF -= OFF2; */ + ao_minus64(&OFF, &OFF, &OFF2); + /**/ + + /* SENS -= SENS2; */ + ao_minus64(&SENS, &SENS, &SENS2); + /**/ } - value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; + /* value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; */ + a.high = 0; + a.low = sample->pres; + ao_mul64(&a, &a, &SENS); + ao_rshift64(&a, &a, 21); + ao_minus64(&a, &a, &OFF); + ao_rshift64(&a, &a, 15); + value->pres = a.low; + /**/ + value->temp = TEMP; -#endif } diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile index afd529f0..984406a9 100644 --- a/src/telemini-v2.0/Makefile +++ b/src/telemini-v2.0/Makefile @@ -19,7 +19,11 @@ INC = \ ao_arch.h \ ao_arch_funcs.h \ cc1111.h \ - ao_product.h + ao_ms5607.h \ + ao_ms5607_convert_8051.c \ + ao_product.h \ + ao_int64.h \ + ao_sample.h CORE_SRC = \ ao_cmd.c \ @@ -37,7 +41,8 @@ CORE_SRC = \ ao_storage.c \ ao_task.c \ ao_telemetry.c \ - ao_freq.c + ao_freq.c \ + ao_int64.c CC1111_SRC = \ ao_adc.c \ diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index 386c8dc3..fad029e2 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -115,6 +115,8 @@ struct ao_adc { #define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) #define AO_SENSE_MAIN(p) ((p)->adc.sense_m) +#define AO_NUM_TASKS 10 + #define AO_ADC_DUMP(p) \ printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \ (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt) diff --git a/src/test/Makefile b/src/test/Makefile index 9c304318..5eee6bbb 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -2,7 +2,8 @@ vpath % ..:../core:../drivers:../util:../micropeak:../aes 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 \ - ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test + ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \ + ao_ms5607_convert_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -76,3 +77,6 @@ ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h cc $(CFLAGS) -o $@ ao_int64_test.c + +ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h + cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c index c26a63b0..8557a1c7 100644 --- a/src/test/ao_int64_test.c +++ b/src/test/ao_int64_test.c @@ -15,6 +15,11 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#define __data +#define __pdata +#define __xdata +#define __reentrant + #include #include #include diff --git a/src/test/ao_ms5607_convert_test.c b/src/test/ao_ms5607_convert_test.c new file mode 100644 index 00000000..ad593204 --- /dev/null +++ b/src/test/ao_ms5607_convert_test.c @@ -0,0 +1,96 @@ +/* + * 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. + */ + +#define __xdata +#define __data +#define __pdata +#define __reentrant + +#include +#include + +struct ao_ms5607_prom ms5607_prom = { + 0x002c, + 0xa6e0, + 0x988e, + 0x6814, + 0x5eff, + 0x8468, + 0x6c86, + 0xa271, +}; + +int32_t D1_mm = 6179630; +int32_t D2_mm = 8933155; + +#include +#define ao_ms5607_convert ao_ms5607_convert_8051 +#include +#include +#include +#include + +struct ao_ms5607_sample ao_sample = { + 6179630, + 8933155 +}; + +int errors; + +void test(int trial, struct ao_ms5607_sample *sample) +{ + struct ao_ms5607_value value, value_8051; + + ao_ms5607_convert(sample, &value); + ao_ms5607_convert_8051(sample, &value_8051); + if (value.temp != value_8051.temp || value.pres != value_8051.pres) { + ++errors; + printf ("trial %d: %d, %d -> %d, %d (should be %d, %d)\n", + trial, + sample->pres, sample->temp, + value_8051.pres, value_8051.temp, + value.pres, value.temp); + } +} + +#define TESTS 10000000 + +#include + +static int32_t rand24(void) { return random() & 0xffffff; } + +int +main(int argc, char **argv) +{ + struct ao_ms5607_sample sample; + int i, start; + + if (argv[1]) + start = atoi(argv[1]); + else + start = 0; + + srandom(10000); + test(-1, &ao_sample); + for (i = 0; i < TESTS; i++) { + sample.pres = rand24(); + sample.temp = rand24(); + if (i >= start) + test(i, &sample); + } + return errors; +} -- cgit v1.2.3 From d54156caf856ab5570f050692b333a2c5d991265 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 26 Aug 2013 18:44:23 -0700 Subject: altos: Make ao_wakeup reentrant In case we end up invoking it from two places at once. Signed-off-by: Keith Packard --- src/core/ao_task.c | 2 +- src/core/ao_task.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_task.c b/src/core/ao_task.c index 0aad6508..18315b1f 100644 --- a/src/core/ao_task.c +++ b/src/core/ao_task.c @@ -420,7 +420,7 @@ ao_sleep(__xdata void *wchan) } void -ao_wakeup(__xdata void *wchan) +ao_wakeup(__xdata void *wchan) __reentrant { #if HAS_TASK_QUEUE struct ao_task *sleep, *next; diff --git a/src/core/ao_task.h b/src/core/ao_task.h index e3a311ed..9c56b480 100644 --- a/src/core/ao_task.h +++ b/src/core/ao_task.h @@ -70,7 +70,7 @@ ao_sleep(__xdata void *wchan); /* Wake all tasks sleeping on wchan */ void -ao_wakeup(__xdata void *wchan); +ao_wakeup(__xdata void *wchan) __reentrant; /* set an alarm to go off in 'delay' ticks */ void -- cgit v1.2.3 From 7b0f9b25a56fa8b4aa1c2e9d79c43e6a97cab0c0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 20 Aug 2013 11:40:17 -0700 Subject: altos: Initial TeleMetrum v2.0 bits Adds new telemetry and logging formats along with code for TeleMetrum v2.0 design. Signed-off-by: Keith Packard --- src/core/ao_gps_report_metrum.c | 110 ++++++++++++++ src/core/ao_log.h | 66 +++++++++ src/core/ao_log_metrum.c | 175 ++++++++++++++++++++++ src/core/ao_telemetry.c | 66 +++++++++ src/core/ao_telemetry.h | 44 ++++++ src/stm/ao_arch.h | 2 + src/stm/ao_beep_stm.c | 48 +++++- src/stm/stm32l.h | 2 +- src/telemetrum-v2.0/.gitignore | 2 + src/telemetrum-v2.0/Makefile | 122 ++++++++++++++++ src/telemetrum-v2.0/ao_pins.h | 282 ++++++++++++++++++++++++++++++++++++ src/telemetrum-v2.0/ao_telemetrum.c | 90 ++++++++++++ 12 files changed, 1004 insertions(+), 5 deletions(-) create mode 100644 src/core/ao_gps_report_metrum.c create mode 100644 src/core/ao_log_metrum.c create mode 100644 src/telemetrum-v2.0/.gitignore create mode 100644 src/telemetrum-v2.0/Makefile create mode 100644 src/telemetrum-v2.0/ao_pins.h create mode 100644 src/telemetrum-v2.0/ao_telemetrum.c (limited to 'src/core') diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c new file mode 100644 index 00000000..4fefe223 --- /dev/null +++ b/src/core/ao_gps_report_metrum.c @@ -0,0 +1,110 @@ +/* + * 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; + uint8_t date_reported = 0; + + for (;;) { + ao_sleep(&ao_gps_data); + ao_mutex_get(&ao_gps_mutex); + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + ao_mutex_put(&ao_gps_mutex); + + if (!(gps_data.flags & AO_GPS_VALID)) + continue; + + 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); + } +} + +void +ao_gps_tracking_report_metrum(void) +{ + static __xdata struct ao_log_metrum gps_log; + static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t c, n, i, p, valid, packets; + uint8_t svid; + + for (;;) { + ao_sleep(&ao_gps_tracking_data); + ao_mutex_get(&ao_gps_mutex); + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_mutex_put(&ao_gps_mutex); + + if (!(n = gps_tracking_data.channels)) + continue; + + 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; +__xdata struct ao_task ao_gps_tracking_report_metrum_task; + +void +ao_gps_report_metrum_init(void) +{ + ao_add_task(&ao_gps_report_metrum_task, + ao_gps_report_metrum, + "gps_report"); + ao_add_task(&ao_gps_tracking_report_metrum_task, + ao_gps_tracking_report_metrum, + "gps_tracking_report"); +} diff --git a/src/core/ao_log.h b/src/core/ao_log.h index dce12f02..f6ab4520 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -45,6 +45,7 @@ extern __pdata enum ao_flight_state ao_log_state; #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_MINI 6 /* 16-byte MS5607 baro only */ +#define AO_LOG_FORMAT_TELEMETRUM 7 /* 16-byte typed telemetrum records */ #define AO_LOG_FORMAT_NONE 127 /* No log at all */ extern __code uint8_t ao_log_format; @@ -135,6 +136,7 @@ ao_log_full(void); #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) @@ -263,6 +265,64 @@ struct ao_log_mega { } 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 */ + } flight; /* 12 */ + /* 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 */ @@ -303,10 +363,16 @@ 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_metrum.c b/src/core/ao_log_metrum.c new file mode 100644 index 00000000..9c17eeed --- /dev/null +++ b/src/core/ao_log_metrum.c @@ -0,0 +1,175 @@ +/* + * 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; + 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 + 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 + log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); + 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[0]; + log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense[1]; + 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_telemetry.c b/src/core/ao_telemetry.c index 03a8a273..fb40451c 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -40,6 +40,10 @@ static __pdata uint16_t ao_aprs_time; #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 @@ -171,6 +175,57 @@ ao_send_mega_data(void) } #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; + telemetry.metrum_sensor.accel = ao_data_accel(packet); + 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[0]; + telemetry.metrum_sensor.sense_m = packet->adc.sense[1]; + + 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)]; + uint8_t i; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_MEGA_DATA; + + telemetry.metrum_data.ground_pres = ao_ground_pres; + 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; + + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_metrum_data_cur = ao_telemetry_metrum_data_max; + } +} +#endif /* AO_SEND_MEGA */ + #ifdef AO_SEND_ALL_BARO static uint8_t ao_baro_sample; @@ -326,10 +381,15 @@ ao_telemetry(void) #ifdef AO_SEND_MEGA ao_send_mega_sensor(); ao_send_mega_data(); +#else +#ifdef AO_SEND_METRUM + ao_send_metrum_sensor(); + ao_send_metrum_data(); #else ao_send_sensor(); #endif #endif +#endif #if HAS_COMPANION if (ao_companion_running) @@ -399,6 +459,12 @@ ao_telemetry_set_interval(uint16_t interval) 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) diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h index f2d201de..17dc3e93 100644 --- a/src/core/ao_telemetry.h +++ b/src/core/ao_telemetry.h @@ -207,6 +207,48 @@ struct ao_telemetry_mega_data { }; +#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_SEND_ALL_BARO */ #define AO_TELEMETRY_BARO 0x80 @@ -240,6 +282,8 @@ union ao_telemetry_all { 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_baro baro; }; diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h index adc288c3..42fe727a 100644 --- a/src/stm/ao_arch.h +++ b/src/stm/ao_arch.h @@ -34,6 +34,8 @@ #define AO_TICK_SIGNED int16_t #endif +#define AO_PORT_TYPE uint16_t + /* Various definitions to make GCC look more like SDCC */ #define ao_arch_naked_declare __attribute__((naked)) diff --git a/src/stm/ao_beep_stm.c b/src/stm/ao_beep_stm.c index 4761fbfc..a95d869b 100644 --- a/src/stm/ao_beep_stm.c +++ b/src/stm/ao_beep_stm.c @@ -17,6 +17,10 @@ #include "ao.h" +#ifndef BEEPER_CHANNEL +#define BEEPER_CHANNEL 1 +#endif + void ao_beep(uint8_t beep) { @@ -56,6 +60,7 @@ ao_beep(uint8_t beep) * is enabled and active high. */ +#if BEEPER_CHANNEL == 1 stm_tim3.ccmr1 = ((0 << STM_TIM234_CCMR1_OC2CE) | (STM_TIM234_CCMR1_OC2M_FROZEN << STM_TIM234_CCMR1_OC2M) | (0 << STM_TIM234_CCMR1_OC2PE) | @@ -68,7 +73,6 @@ ao_beep(uint8_t beep) (0 << STM_TIM234_CCMR1_OC1FE) | (STM_TIM234_CCMR1_CC1S_OUTPUT << STM_TIM234_CCMR1_CC1S)); - stm_tim3.ccer = ((0 << STM_TIM234_CCER_CC4NP) | (0 << STM_TIM234_CCER_CC4P) | (0 << STM_TIM234_CCER_CC4E) | @@ -81,6 +85,33 @@ ao_beep(uint8_t beep) (0 << STM_TIM234_CCER_CC1NP) | (0 << STM_TIM234_CCER_CC1P) | (1 << STM_TIM234_CCER_CC1E)); +#endif +#if BEEPER_CHANNEL == 4 + stm_tim3.ccmr2 = ((0 << STM_TIM234_CCMR2_OC4CE) | + (STM_TIM234_CCMR2_OC4M_TOGGLE << STM_TIM234_CCMR2_OC4M) | + (0 << STM_TIM234_CCMR2_OC4PE) | + (0 << STM_TIM234_CCMR2_OC4FE) | + (STM_TIM234_CCMR2_CC4S_OUTPUT << STM_TIM234_CCMR2_CC4S) | + + (0 << STM_TIM234_CCMR2_OC3CE) | + (STM_TIM234_CCMR2_OC3M_FROZEN << STM_TIM234_CCMR2_OC3M) | + (0 << STM_TIM234_CCMR2_OC3PE) | + (0 << STM_TIM234_CCMR2_OC3FE) | + (STM_TIM234_CCMR2_CC3S_OUTPUT << STM_TIM234_CCMR2_CC3S)); + + stm_tim3.ccer = ((0 << STM_TIM234_CCER_CC4NP) | + (0 << STM_TIM234_CCER_CC4P) | + (1 << STM_TIM234_CCER_CC4E) | + (0 << STM_TIM234_CCER_CC3NP) | + (0 << STM_TIM234_CCER_CC3P) | + (0 << STM_TIM234_CCER_CC3E) | + (0 << STM_TIM234_CCER_CC2NP) | + (0 << STM_TIM234_CCER_CC2P) | + (0 << STM_TIM234_CCER_CC2E) | + (0 << STM_TIM234_CCER_CC1NP) | + (0 << STM_TIM234_CCER_CC1P) | + (0 << STM_TIM234_CCER_CC1E)); +#endif /* 5. Enable the counter by setting the CEN bit in the TIMx_CR1 register. */ @@ -110,13 +141,22 @@ ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant void ao_beep_init(void) { - /* Our beeper is on PC6, which is hooked to TIM3_CH1, - * which is on PC6 - */ +#if BEEPER_CHANNEL == 1 + /* Our beeper is on PC6, which is hooked to TIM3_CH1. + */ stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOCEN); stm_afr_set(&stm_gpioc, 6, STM_AFR_AF2); +#endif +#if BEEPER_CHANNEL == 4 + + /* Our beeper is on PB1, which is hooked to TIM3_CH4. + */ + stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOBEN); + + stm_afr_set(&stm_gpiob, 1, STM_AFR_AF2); +#endif /* Leave the timer off until requested */ diff --git a/src/stm/stm32l.h b/src/stm/stm32l.h index 1868468f..ff3f5336 100644 --- a/src/stm/stm32l.h +++ b/src/stm/stm32l.h @@ -1739,7 +1739,7 @@ extern struct stm_tim234 stm_tim2, stm_tim3, stm_tim4; #define STM_TIM234_CCMR1_CC1S_INPUT_TRC 3 #define STM_TIM234_CCMR1_CC1S_MASK 3 -#define STM_TIM234_CCMR2_OC2CE 15 +#define STM_TIM234_CCMR2_OC4CE 15 #define STM_TIM234_CCMR2_OC4M 12 #define STM_TIM234_CCMR2_OC4M_FROZEN 0 #define STM_TIM234_CCMR2_OC4M_SET_HIGH_ON_MATCH 1 diff --git a/src/telemetrum-v2.0/.gitignore b/src/telemetrum-v2.0/.gitignore new file mode 100644 index 00000000..35ce24d4 --- /dev/null +++ b/src/telemetrum-v2.0/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +telemetrum-*.elf diff --git a/src/telemetrum-v2.0/Makefile b/src/telemetrum-v2.0/Makefile new file mode 100644 index 00000000..c03a5539 --- /dev/null +++ b/src/telemetrum-v2.0/Makefile @@ -0,0 +1,122 @@ +# +# AltOS build +# +# + +include ../stm/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_companion.h \ + ao_data.h \ + ao_sample.h \ + ao_pins.h \ + altitude-pa.h \ + ao_kalman.h \ + ao_product.h \ + ao_ms5607.h \ + ao_mma655x.h \ + ao_cc1120_CC1120.h \ + ao_profile.h \ + ao_task.h \ + ao_whiten.h \ + ao_sample_profile.h \ + ao_mpu.h \ + stm32l.h \ + Makefile + +#PROFILE=ao_profile.c +#PROFILE_DEF=-DAO_PROFILE=1 + +#SAMPLE_PROFILE=ao_sample_profile.c \ +# ao_sample_profile_timer.c +#SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1 + +#STACK_GUARD=ao_mpu_stm.c +#STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 + +ALTOS_SRC = \ + ao_boot_chain.c \ + ao_interrupt.c \ + ao_product.c \ + ao_romconfig.c \ + ao_cmd.c \ + ao_config.c \ + ao_task.c \ + ao_led.c \ + ao_stdio.c \ + ao_panic.c \ + ao_timer.c \ + ao_mutex.c \ + ao_serial_stm.c \ + ao_gps_ublox.c \ + ao_gps_show.c \ + ao_gps_report_metrum.c \ + ao_ignite.c \ + ao_freq.c \ + ao_dma_stm.c \ + ao_spi_stm.c \ + ao_cc1120.c \ + ao_fec_tx.c \ + ao_fec_rx.c \ + ao_data.c \ + ao_ms5607.c \ + ao_mma655x.c \ + ao_adc_stm.c \ + ao_beep_stm.c \ + ao_storage.c \ + ao_m25.c \ + ao_usb_stm.c \ + ao_exti_stm.c \ + ao_report.c \ + ao_convert_pa.c \ + ao_log.c \ + ao_log_metrum.c \ + ao_sample.c \ + ao_kalman.c \ + ao_flight.c \ + ao_telemetry.c \ + ao_packet_slave.c \ + ao_packet.c \ + ao_companion.c \ + ao_aprs.c \ + $(PROFILE) \ + $(SAMPLE_PROFILE) \ + $(STACK_GUARD) + +PRODUCT=TeleMetrum-v2.0 +PRODUCT_DEF=-DTELEMETRUM_V_2_0 +IDPRODUCT=0x000b + +CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) -Os -g + +PROGNAME=telemetrum-v2.0 +PROG=$(PROGNAME)-$(VERSION).elf + +SRC=$(ALTOS_SRC) ao_telemetrum.c +OBJ=$(SRC:.c=.o) + +all: $(PROG) + +$(PROG): Makefile $(OBJ) altos.ld + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + +../altitude-pa.h: make-altitude-pa + nickle $< > $@ + +$(OBJ): $(INC) + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +distclean: clean + +clean: + rm -f *.o $(PROGNAME)-*.elf + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/telemetrum-v2.0/ao_pins.h b/src/telemetrum-v2.0/ao_pins.h new file mode 100644 index 00000000..36cfc7e0 --- /dev/null +++ b/src/telemetrum-v2.0/ao_pins.h @@ -0,0 +1,282 @@ +/* + * 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_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_TASK_QUEUE 1 + +/* 8MHz High speed external crystal */ +#define AO_HSE 8000000 + +/* PLLVCO = 96MHz (so that USB will work) */ +#define AO_PLLMUL 12 +#define AO_RCC_CFGR_PLLMUL (STM_RCC_CFGR_PLLMUL_12) + +/* SYSCLK = 32MHz (no need to go faster than CPU) */ +#define AO_PLLDIV 3 +#define AO_RCC_CFGR_PLLDIV (STM_RCC_CFGR_PLLDIV_3) + +/* HCLK = 32MHz (CPU clock) */ +#define AO_AHB_PRESCALER 1 +#define AO_RCC_CFGR_HPRE_DIV STM_RCC_CFGR_HPRE_DIV_1 + +/* Run APB1 at 16MHz (HCLK/2) */ +#define AO_APB1_PRESCALER 2 +#define AO_RCC_CFGR_PPRE1_DIV STM_RCC_CFGR_PPRE2_DIV_2 + +/* Run APB2 at 16MHz (HCLK/2) */ +#define AO_APB2_PRESCALER 2 +#define AO_RCC_CFGR_PPRE2_DIV STM_RCC_CFGR_PPRE2_DIV_2 + +#define HAS_SERIAL_1 0 +#define USE_SERIAL_1_STDIN 0 +#define SERIAL_1_PB6_PB7 0 +#define SERIAL_1_PA9_PA10 1 + +#define HAS_SERIAL_2 0 +#define USE_SERIAL_2_STDIN 0 +#define SERIAL_2_PA2_PA3 0 +#define SERIAL_2_PD5_PD6 0 + +#define HAS_SERIAL_3 1 +#define USE_SERIAL_3_STDIN 0 +#define SERIAL_3_PB10_PB11 1 +#define SERIAL_3_PC10_PC11 0 +#define SERIAL_3_PD8_PD9 0 + +#define ao_gps_getchar ao_serial3_getchar +#define ao_gps_putchar ao_serial3_putchar +#define ao_gps_set_speed ao_serial3_set_speed +#define ao_gps_fifo (ao_stm_usart3.rx_fifo) + +#define HAS_EEPROM 1 +#define USE_INTERNAL_FLASH 0 +#define HAS_USB 1 +#define HAS_BEEP 1 +#define BEEPER_CHANNEL 4 +#define HAS_RADIO 1 +#define HAS_TELEMETRY 1 +#define HAS_APRS 1 + +#define HAS_SPI_1 1 +#define SPI_1_PA5_PA6_PA7 1 /* Barometer */ +#define SPI_1_PB3_PB4_PB5 1 /* Accelerometer */ +#define SPI_1_PE13_PE14_PE15 0 +#define SPI_1_OSPEEDR STM_OSPEEDR_10MHz + +#define HAS_SPI_2 1 +#define SPI_2_PB13_PB14_PB15 1 /* Flash, Companion, Radio */ +#define SPI_2_PD1_PD3_PD4 0 +#define SPI_2_OSPEEDR STM_OSPEEDR_10MHz + +#define SPI_2_PORT (&stm_gpiob) +#define SPI_2_SCK_PIN 13 +#define SPI_2_MISO_PIN 14 +#define SPI_2_MOSI_PIN 15 + +#define HAS_I2C_1 0 +#define I2C_1_PB8_PB9 0 + +#define HAS_I2C_2 0 +#define I2C_2_PB10_PB11 0 + +#define PACKET_HAS_SLAVE 1 +#define PACKET_HAS_MASTER 0 + +#define LOW_LEVEL_DEBUG 0 + +#define LED_PORT_ENABLE STM_RCC_AHBENR_GPIOCEN +#define LED_PORT (&stm_gpioc) +#define LED_PIN_RED 14 +#define LED_PIN_GREEN 15 +#define AO_LED_RED (1 << LED_PIN_RED) +#define AO_LED_GREEN (1 << LED_PIN_GREEN) + +#define LEDS_AVAILABLE (AO_LED_RED | AO_LED_GREEN) + +#define HAS_GPS 1 +#define HAS_FLIGHT 1 +#define HAS_ADC 1 +#define HAS_ADC_TEMP 1 +#define HAS_LOG 1 + +/* + * Igniter + */ + +#define HAS_IGNITE 1 +#define HAS_IGNITE_REPORT 1 + +#define AO_SENSE_DROGUE(p) ((p)->adc.sense[0]) +#define AO_SENSE_MAIN(p) ((p)->adc.sense[1]) +#define AO_IGNITER_CLOSED 400 +#define AO_IGNITER_OPEN 60 + +/* Drogue */ +#define AO_IGNITER_DROGUE_PORT (&stm_gpioa) +#define AO_IGNITER_DROGUE_PIN 8 + +/* Main */ +#define AO_IGNITER_MAIN_PORT (&stm_gpioa) +#define AO_IGNITER_MAIN_PIN 9 + +#define AO_IGNITER_SET_DROGUE(v) stm_gpio_set(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, v) +#define AO_IGNITER_SET_MAIN(v) stm_gpio_set(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, v) + +/* + * ADC + */ +#define AO_DATA_RING 32 +#define AO_ADC_NUM_SENSE 2 + +struct ao_adc { + int16_t sense[AO_ADC_NUM_SENSE]; + int16_t v_batt; + int16_t temp; +}; + +#define AO_ADC_DUMP(p) \ + printf("tick: %5u drogue: %5d main: %5d batt: %5d pbatt: %5d temp: %5d\n", \ + (p)->tick, \ + (p)->adc.sense[0], (p)->adc.sense[1], \ + (p)->adc.v_batt, (p)->adc.temp) + +#define AO_ADC_SENSE_DROGUE 0 +#define AO_ADC_SENSE_DROGUE_PORT (&stm_gpioa) +#define AO_ADC_SENSE_DROGUE_PIN 0 + +#define AO_ADC_SENSE_MAIN 1 +#define AO_ADC_SENSE_MAIN_PORT (&stm_gpioa) +#define AO_ADC_SENSE_MAIN_PIN 1 + +#define AO_ADC_V_BATT 8 +#define AO_ADC_V_BATT_PORT (&stm_gpiob) +#define AO_ADC_V_BATT_PIN 0 + +#define AO_ADC_TEMP 16 + +#define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ + (1 << STM_RCC_AHBENR_GPIOEEN) | \ + (1 << STM_RCC_AHBENR_GPIOBEN)) + +#define AO_NUM_ADC_PIN 3 + +#define AO_ADC_PIN0_PORT AO_ADC_SENSE_DROGUE_PORT +#define AO_ADC_PIN0_PIN AO_ADC_SENSE_DROGUE_PIN +#define AO_ADC_PIN1_PORT AO_ADC_SENSE_MAIN_PORT +#define AO_ADC_PIN1_PIN AO_ADC_SENSE_MAIN_PIN +#define AO_ADC_PIN2_PORT AO_ADC_V_BATT_PORT +#define AO_ADC_PIN2_PIN AO_ADC_V_BATT_PIN + +#define AO_NUM_ADC (AO_NUM_ADC_PIN + 1) + +#define AO_ADC_SQ1 AO_ADC_SENSE_DROGUE +#define AO_ADC_SQ2 AO_ADC_SENSE_MAIN +#define AO_ADC_SQ3 AO_ADC_V_BATT +#define AO_ADC_SQ4 AO_ADC_TEMP + +/* + * Pressure sensor settings + */ +#define HAS_MS5607 1 +#define HAS_MS5611 0 +#define AO_MS5607_PRIVATE_PINS 1 +#define AO_MS5607_CS_PORT (&stm_gpiob) +#define AO_MS5607_CS_PIN 12 +#define AO_MS5607_CS_MASK (1 << AO_MS5607_CS) +#define AO_MS5607_MISO_PORT (&stm_gpioa) +#define AO_MS5607_MISO_PIN 6 +#define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO) +#define AO_MS5607_SPI_INDEX AO_SPI_1_PA5_PA6_PA7 + +/* + * SPI Flash memory + */ + +#define M25_MAX_CHIPS 1 +#define AO_M25_SPI_CS_PORT (&stm_gpiob) +#define AO_M25_SPI_CS_MASK (1 << 8) +#define AO_M25_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + +/* + * Radio (cc1120) + */ + +/* gets pretty close to 434.550 */ + +#define AO_RADIO_CAL_DEFAULT 0x6ca333 + +#define AO_FEC_DEBUG 0 +#define AO_CC1120_SPI_CS_PORT (&stm_gpioa) +#define AO_CC1120_SPI_CS_PIN 2 +#define AO_CC1120_SPI_BUS AO_SPI_2_PB13_PB14_PB15 +#define AO_CC1120_SPI stm_spi2 + +#define AO_CC1120_INT_PORT (&stm_gpioa) +#define AO_CC1120_INT_PIN (3) +#define AO_CC1120_MCU_WAKEUP_PORT (&stm_gpioa) +#define AO_CC1120_MCU_WAKEUP_PIN (4) + +#define AO_CC1120_INT_GPIO 2 +#define AO_CC1120_INT_GPIO_IOCFG CC1120_IOCFG2 + +#define AO_CC1120_MARC_GPIO 3 +#define AO_CC1120_MARC_GPIO_IOCFG CC1120_IOCFG3 + +#define HAS_BOOT_RADIO 0 + +#define HAS_HIGHG_ACCEL 1 + +/* + * mma655x + */ + +#define HAS_MMA655X 1 +#define AO_MMA655X_SPI_INDEX AO_SPI_1_PE13_PE14_PE15 +#define AO_MMA655X_CS_PORT (&stm_gpiod) +#define AO_MMA655X_CS_PIN 4 + +#define NUM_CMDS 16 + +/* + * Companion + */ + +#define AO_COMPANION_CS_PORT (&stm_gpiob) +#define AO_COMPANION_CS_PIN (6) +#define AO_COMPANION_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + +/* + * Monitor + */ + +#define HAS_MONITOR 0 +#define LEGACY_MONITOR 0 +#define HAS_MONITOR_PUT 1 +#define AO_MONITOR_LED 0 +#define HAS_RSSI 0 + +/* + * Profiling Viterbi decoding + */ + +#ifndef AO_PROFILE +#define AO_PROFILE 0 +#endif + +#endif /* _AO_PINS_H_ */ diff --git a/src/telemetrum-v2.0/ao_telemetrum.c b/src/telemetrum-v2.0/ao_telemetrum.c new file mode 100644 index 00000000..e365417d --- /dev/null +++ b/src/telemetrum-v2.0/ao_telemetrum.c @@ -0,0 +1,90 @@ +/* + * 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 +#include +#include +#include +#include +#include +#if HAS_SAMPLE_PROFILE +#include +#endif +#if HAS_STACK_GUARD +#include +#endif + +int +main(void) +{ + ao_clock_init(); + +#if HAS_STACK_GUARD + ao_mpu_init(); +#endif + + ao_task_init(); + ao_serial_init(); + ao_led_init(LEDS_AVAILABLE); + ao_led_on(AO_LED_GREEN); + ao_timer_init(); + + ao_spi_init(); + ao_dma_init(); + ao_exti_init(); + + ao_adc_init(); +#if HAS_BEEP + ao_beep_init(); +#endif + ao_cmd_init(); + +#if HAS_MS5607 + ao_ms5607_init(); +#endif +#if HAS_MMA655X + ao_mma655x_init(); +#endif + + ao_storage_init(); + + ao_flight_init(); + ao_log_init(); + ao_report_init(); + + ao_usb_init(); + ao_gps_init(); + ao_gps_report_metrum_init(); + ao_telemetry_init(); + ao_radio_init(); + ao_packet_slave_init(FALSE); + ao_igniter_init(); + ao_companion_init(); + + ao_config_init(); +#if AO_PROFILE + ao_profile_init(); +#endif +#if HAS_SAMPLE_PROFILE + ao_sample_profile_init(); +#endif + + ao_start_scheduler(); + return 0; +} -- cgit v1.2.3 From 454a41359b94e9bcf8582420abc359bbab9d8176 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 23 Aug 2013 11:25:56 -0700 Subject: altos: Rename TeleMetrum v2.0 ADC sense members Use sense_a and sense_m instead of sense[2] Signed-off-by: Keith Packard --- src/core/ao_log_metrum.c | 4 ++-- src/core/ao_telemetry.c | 4 ++-- src/telemetrum-v2.0/ao_pins.h | 13 +++++++------ 3 files changed, 11 insertions(+), 10 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_log_metrum.c b/src/core/ao_log_metrum.c index 9c17eeed..43441e7a 100644 --- a/src/core/ao_log_metrum.c +++ b/src/core/ao_log_metrum.c @@ -127,8 +127,8 @@ ao_log(void) 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[0]; - log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense[1]; + 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; } diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index fb40451c..65d7d08f 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -195,8 +195,8 @@ ao_send_metrum_sensor(void) telemetry.metrum_sensor.height = ao_height; telemetry.metrum_sensor.v_batt = packet->adc.v_batt; - telemetry.metrum_sensor.sense_a = packet->adc.sense[0]; - telemetry.metrum_sensor.sense_m = packet->adc.sense[1]; + telemetry.metrum_sensor.sense_a = packet->adc.sense_a; + telemetry.metrum_sensor.sense_m = packet->adc.sense_m; ao_radio_send(&telemetry, sizeof (telemetry)); } diff --git a/src/telemetrum-v2.0/ao_pins.h b/src/telemetrum-v2.0/ao_pins.h index 36cfc7e0..c4f8c696 100644 --- a/src/telemetrum-v2.0/ao_pins.h +++ b/src/telemetrum-v2.0/ao_pins.h @@ -122,8 +122,8 @@ #define HAS_IGNITE 1 #define HAS_IGNITE_REPORT 1 -#define AO_SENSE_DROGUE(p) ((p)->adc.sense[0]) -#define AO_SENSE_MAIN(p) ((p)->adc.sense[1]) +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) #define AO_IGNITER_CLOSED 400 #define AO_IGNITER_OPEN 60 @@ -145,16 +145,17 @@ #define AO_ADC_NUM_SENSE 2 struct ao_adc { - int16_t sense[AO_ADC_NUM_SENSE]; + int16_t sense_a; + int16_t sense_m; int16_t v_batt; int16_t temp; }; #define AO_ADC_DUMP(p) \ - printf("tick: %5u drogue: %5d main: %5d batt: %5d pbatt: %5d temp: %5d\n", \ + printf("tick: %5u drogue: %5d main: %5d batt: %5d\n", \ (p)->tick, \ - (p)->adc.sense[0], (p)->adc.sense[1], \ - (p)->adc.v_batt, (p)->adc.temp) + (p)->adc.sense_a, (p)->adc.sense_m, \ + (p)->adc.v_batt); #define AO_ADC_SENSE_DROGUE 0 #define AO_ADC_SENSE_DROGUE_PORT (&stm_gpioa) -- cgit v1.2.3 From 39475c7b8da4f29936f73ffa2bff112f50ee9328 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 28 Aug 2013 21:52:29 -0600 Subject: altos: TM v2 places the MMA6555 upside down compared to Tmega Means we need to invert the data coming out to make it work Signed-off-by: Keith Packard --- src/core/ao_data.h | 4 ++++ src/telemetrum-v2.0/ao_pins.h | 1 + 2 files changed, 5 insertions(+) (limited to 'src/core') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index b0f086f8..c873e9d3 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -272,7 +272,11 @@ typedef int16_t accel_t; /* MMA655X is hooked up so that positive values represent negative acceleration */ #define ao_data_accel(packet) ((packet)->mma655x) +#if AO_MMA655X_INVERT +#define ao_data_accel_cook(packet) (4095 - (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) (4095 - (accel)) diff --git a/src/telemetrum-v2.0/ao_pins.h b/src/telemetrum-v2.0/ao_pins.h index 04cce99e..a7236b80 100644 --- a/src/telemetrum-v2.0/ao_pins.h +++ b/src/telemetrum-v2.0/ao_pins.h @@ -257,6 +257,7 @@ struct ao_adc { #define AO_MMA655X_SPI_INDEX AO_SPI_1_PB3_PB4_PB5 #define AO_MMA655X_CS_PORT (&stm_gpiob) #define AO_MMA655X_CS_PIN 9 +#define AO_MMA655X_INVERT 1 #define NUM_CMDS 16 -- cgit v1.2.3 From 44d4c66b21d6b5a0c656fdff6d01ef1d125c1101 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 28 Aug 2013 21:54:31 -0600 Subject: altos: Update time for next alarm each time a task is added Adding a task with a sooner timeout than existing alarm tasks was not correctly updating the time to fire the next alarm, causing tasks to be delayed by the wrong amount. Signed-off-by: Keith Packard --- src/core/ao_sample_profile.c | 6 +++--- src/core/ao_task.c | 3 +++ 2 files changed, 6 insertions(+), 3 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_sample_profile.c b/src/core/ao_sample_profile.c index 1d9ed414..e682bd98 100644 --- a/src/core/ao_sample_profile.c +++ b/src/core/ao_sample_profile.c @@ -20,15 +20,15 @@ #include #ifndef AO_SAMPLE_PROFILE_LOW_PC -#define AO_SAMPLE_PROFILE_LOW_PC 0x08000000 +#define AO_SAMPLE_PROFILE_LOW_PC 0x08002000 #endif #ifndef AO_SAMPLE_PROFILE_HIGH_PC -#define AO_SAMPLE_PROFILE_HIGH_PC (AO_SAMPLE_PROFILE_LOW_PC + 44 * 1024) +#define AO_SAMPLE_PROFILE_HIGH_PC 0x08003000 #endif #ifndef AO_SAMPLE_PROFILE_SHIFT -#define AO_SAMPLE_PROFILE_SHIFT 6 +#define AO_SAMPLE_PROFILE_SHIFT 3 #endif #define AO_SAMPLE_PROFILE_RANGE (AO_SAMPLE_PROFILE_HIGH_PC - AO_SAMPLE_PROFILE_LOW_PC) diff --git a/src/core/ao_task.c b/src/core/ao_task.c index 0aad6508..4f48e32d 100644 --- a/src/core/ao_task.c +++ b/src/core/ao_task.c @@ -109,6 +109,8 @@ ao_task_validate_alarm_queue(void) 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() @@ -123,6 +125,7 @@ ao_task_to_alarm_queue(struct ao_task *task) 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; } -- cgit v1.2.3 From ce1378385ef273010498e81c205f42d8e32c7dc1 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 29 Aug 2013 19:22:18 -0500 Subject: altos: Split EasyMini and TeleMini log formats Same data, but EasyMini uses a 3.0V supply while TeleMini uses 3.3V, which changes the intepretation of all of the ADC values Signed-off-by: Keith Packard --- src/core/ao_log.h | 3 ++- src/core/ao_log_mini.c | 2 +- src/easymini-v0.1/ao_pins.h | 2 ++ src/telemini-v2.0/ao_pins.h | 1 + 4 files changed, 6 insertions(+), 2 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_log.h b/src/core/ao_log.h index f6ab4520..a2f342d7 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -44,8 +44,9 @@ extern __pdata enum ao_flight_state ao_log_state; #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_MINI 6 /* 16-byte MS5607 baro only */ +#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; diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c index 46b285f3..99a85982 100644 --- a/src/core/ao_log_mini.c +++ b/src/core/ao_log_mini.c @@ -23,7 +23,7 @@ static __xdata uint8_t ao_log_mutex; static __xdata struct ao_log_mini log; -__code uint8_t ao_log_format = AO_LOG_FORMAT_MINI; +__code uint8_t ao_log_format = AO_LOG_FORMAT; static uint8_t ao_log_csum(__xdata uint8_t *b) __reentrant diff --git a/src/easymini-v0.1/ao_pins.h b/src/easymini-v0.1/ao_pins.h index c09fb4c2..e0eb10bf 100644 --- a/src/easymini-v0.1/ao_pins.h +++ b/src/easymini-v0.1/ao_pins.h @@ -48,6 +48,8 @@ #define PACKET_HAS_SLAVE 0 +#define AO_LOG_FORMAT AO_LOG_FORMAT_EASYMINI + /* USART */ #define HAS_SERIAL 0 diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index 264ad16d..c4681ee2 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -102,6 +102,7 @@ #define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) #define AO_SEND_MINI +#define AO_LOG_FORMAT AO_LOG_FORMAT_TELEMINI /* * ADC -- cgit v1.2.3 From 77dc89ed5b7bf8f5b3fa3b6131660f1a98f583ea Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 31 Aug 2013 23:11:39 -0500 Subject: altoslib/altosui: Further AltosState transition work Parses most eeprom and telem records now; altosui updated to show from AltosState info. Signed-off-by: Keith Packard --- altoslib/AltosConvert.java | 22 ++ altoslib/AltosEeprom.java | 45 +++- altoslib/AltosEepromChunk.java | 26 +++ altoslib/AltosEepromFile.java | 5 + altoslib/AltosEepromGPS.java | 178 ++++++++++++++++ altoslib/AltosEepromHeader.java | 6 +- altoslib/AltosEepromMega.java | 262 +++++++++++------------ altoslib/AltosEepromMetrum.java | 2 + altoslib/AltosEepromMetrum2.java | 178 ++++++++++++++++ altoslib/AltosEepromMini.java | 20 +- altoslib/AltosEepromTM.java | 14 +- altoslib/AltosEepromTeleScience.java | 2 + altoslib/AltosGPS.java | 10 +- altoslib/AltosLib.java | 1 + altoslib/AltosState.java | 129 +++++++++--- altoslib/AltosTelemetry.java | 44 +--- altoslib/AltosTelemetryConfiguration.java | 55 +++++ altoslib/AltosTelemetryLegacy.java | 2 +- altoslib/AltosTelemetryLocation.java | 88 ++++++++ altoslib/AltosTelemetryMegaData.java | 85 ++++++++ altoslib/AltosTelemetryMegaSensor.java | 84 ++++++++ altoslib/AltosTelemetryMetrumData.java | 42 ++++ altoslib/AltosTelemetryMetrumSensor.java | 69 ++++++ altoslib/AltosTelemetryRaw.java | 28 +++ altoslib/AltosTelemetrySatellite.java | 50 +++++ altoslib/AltosTelemetrySensor.java | 80 +++++++ altoslib/AltosTelemetryStandard.java | 106 ++++++++++ altoslib/Makefile.am | 34 ++- altosui/AltosAscent.java | 4 +- altosui/AltosDescent.java | 4 +- altosui/AltosDisplayThread.java | 2 +- altosui/AltosEepromDownload.java | 340 ++++-------------------------- altosui/AltosFlightStatus.java | 2 +- altosui/AltosFlightUI.java | 2 +- altosui/AltosIdleMonitorUI.java | 8 +- altosui/AltosInfoTable.java | 30 +-- altosui/AltosLanded.java | 5 +- altosui/AltosPad.java | 74 ++++--- altosui/AltosUI.java | 15 +- src/core/ao_log.h | 3 +- 40 files changed, 1552 insertions(+), 604 deletions(-) create mode 100644 altoslib/AltosEepromGPS.java create mode 100644 altoslib/AltosEepromMetrum2.java create mode 100644 altoslib/AltosTelemetryConfiguration.java create mode 100644 altoslib/AltosTelemetryLocation.java create mode 100644 altoslib/AltosTelemetryMegaData.java create mode 100644 altoslib/AltosTelemetryMegaSensor.java create mode 100644 altoslib/AltosTelemetryMetrumData.java create mode 100644 altoslib/AltosTelemetryMetrumSensor.java create mode 100644 altoslib/AltosTelemetryRaw.java create mode 100644 altoslib/AltosTelemetrySatellite.java create mode 100644 altoslib/AltosTelemetrySensor.java create mode 100644 altoslib/AltosTelemetryStandard.java (limited to 'src/core') diff --git a/altoslib/AltosConvert.java b/altoslib/AltosConvert.java index a1e2cdca..cf2bc59f 100644 --- a/altoslib/AltosConvert.java +++ b/altoslib/AltosConvert.java @@ -196,6 +196,28 @@ public class AltosConvert { return ((count / 16.0) / 2047.0 + 0.095) / 0.009 * 1000.0; } + static double + thermometer_to_temperature(double thermo) + { + return (thermo - 19791.268) / 32728.0 * 1.25 / 0.00247; + } + + static double mega_adc(int raw) { + return raw / 4095.0; + } + + static public double mega_battery_voltage(int v_batt) { + if (v_batt != AltosRecord.MISSING) + return 3.3 * mega_adc(v_batt) * (15.0 + 27.0) / 27.0; + return AltosRecord.MISSING; + } + + static double mega_pyro_voltage(int raw) { + if (raw != AltosRecord.MISSING) + return 3.3 * mega_adc(raw) * (100.0 + 27.0) / 27.0; + return AltosRecord.MISSING; + } + public static double radio_to_frequency(int freq, int setting, int cal, int channel) { double f; diff --git a/altoslib/AltosEeprom.java b/altoslib/AltosEeprom.java index 31646c7e..081b3be1 100644 --- a/altoslib/AltosEeprom.java +++ b/altoslib/AltosEeprom.java @@ -27,8 +27,26 @@ public abstract class AltosEeprom implements AltosStateUpdate { public int data8[]; public boolean valid; + public int data8(int i) { + return data8[i]; + } + + public int data16(int i) { + return ((data8[i] | (data8[i+1] << 8)) << 16) >> 16; + } + + public int data24(int i) { + return data8[i] | (data8[i+1] << 8) | (data8[i+2] << 16); + } + + public int data32(int i) { + return data8[i] | (data8[i+1] << 8) | (data8[i+2] << 16) | (data8[i+3] << 24); + } + public final static int header_length = 4; + public abstract int record_length(); + public abstract void update_state(AltosState state); public void write(PrintStream out) { @@ -40,14 +58,28 @@ public abstract class AltosEeprom implements AltosStateUpdate { out.printf ("\n"); } - void parse_chunk(AltosEepromChunk chunk, int start, int record_length) throws ParseException { + public String string() { + String s; + + s = String.format("%c %04x", cmd, tick); + if (data8 != null) { + for (int i = 0; i < data8.length; i++) { + String d = String.format(" %02x", data8[i]); + s = s.concat(d); + } + } + s = s.concat("\n"); + return s; + } + + void parse_chunk(AltosEepromChunk chunk, int start) throws ParseException { cmd = chunk.data(start); - int data_length = record_length - header_length; + int data_length = record_length() - header_length; - valid = !chunk.erased(start, record_length); + valid = !chunk.erased(start, record_length()); if (valid) { - if (AltosConvert.checksum(chunk.data, start, record_length) != 0) + if (AltosConvert.checksum(chunk.data, start, record_length()) != 0) throw new ParseException(String.format("invalid checksum at 0x%x", chunk.address + start), 0); } else { @@ -61,12 +93,12 @@ public abstract class AltosEeprom implements AltosStateUpdate { data8[i] = chunk.data(start + header_length + i); } - void parse_string(String line, int record_length) { + void parse_string(String line) { valid = false; tick = 0; cmd = AltosLib.AO_LOG_INVALID; - int data_length = record_length - header_length; + int data_length = record_length() - header_length; if (line == null) return; @@ -79,6 +111,7 @@ public abstract class AltosEeprom implements AltosStateUpdate { tick = Integer.parseInt(tokens[1],16); valid = true; data8 = new int[data_length]; + for (int i = 0; i < data_length; i++) data8[i] = Integer.parseInt(tokens[2 + i],16); } diff --git a/altoslib/AltosEepromChunk.java b/altoslib/AltosEepromChunk.java index b1bba3bb..1709352b 100644 --- a/altoslib/AltosEepromChunk.java +++ b/altoslib/AltosEepromChunk.java @@ -62,6 +62,32 @@ public class AltosEepromChunk { return true; } + public AltosEeprom eeprom(int offset, int log_format) { + AltosEeprom eeprom = null; + try { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_FULL: + eeprom = new AltosEepromTM(this, offset); + break; + case AltosLib.AO_LOG_FORMAT_TINY: + case AltosLib.AO_LOG_FORMAT_TELEMETRY: + case AltosLib.AO_LOG_FORMAT_TELESCIENCE: + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + eeprom = new AltosEepromMega(this, offset); + break; + case AltosLib.AO_LOG_FORMAT_TELEMETRUM: + eeprom = new AltosEepromMetrum2(this, offset); + break; + case AltosLib.AO_LOG_FORMAT_TELEMINI: + case AltosLib.AO_LOG_FORMAT_EASYMINI: + eeprom = new AltosEepromMini(this, offset); + break; + } + } catch (ParseException e) { + } + return eeprom; + } + public AltosEepromChunk(AltosLink link, int block, boolean flush) throws TimeoutException, InterruptedException { diff --git a/altoslib/AltosEepromFile.java b/altoslib/AltosEepromFile.java index 2f4c54d7..367b6791 100644 --- a/altoslib/AltosEepromFile.java +++ b/altoslib/AltosEepromFile.java @@ -72,6 +72,7 @@ public class AltosEepromFile extends AltosStateIterable { headers = new AltosEepromIterable(AltosEepromHeader.read(input)); start = headers.state(); + start.set_state(AltosLib.ao_flight_pad); switch (start.log_format) { case AltosLib.AO_LOG_FORMAT_FULL: @@ -81,6 +82,10 @@ public class AltosEepromFile extends AltosStateIterable { case AltosLib.AO_LOG_FORMAT_TELEMETRY: case AltosLib.AO_LOG_FORMAT_TELESCIENCE: case AltosLib.AO_LOG_FORMAT_TELEMEGA: + body = new AltosEepromIterable(AltosEepromMega.read(input)); + break; + case AltosLib.AO_LOG_FORMAT_TELEMETRUM: + body = new AltosEepromIterable(AltosEepromMetrum2.read(input)); break; case AltosLib.AO_LOG_FORMAT_TELEMINI: case AltosLib.AO_LOG_FORMAT_EASYMINI: diff --git a/altoslib/AltosEepromGPS.java b/altoslib/AltosEepromGPS.java new file mode 100644 index 00000000..d8e47a6e --- /dev/null +++ b/altoslib/AltosEepromGPS.java @@ -0,0 +1,178 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromGPS extends AltosEeprom { + public static final int record_length = 16; + + public int record_length() { return record_length; } + + /* AO_LOG_FLIGHT elements */ + public int flight() { return data16(0); } + public int ground_accel() { return data16(2); } + public int ground_pres() { return data32(4); } + public int ground_temp() { return data32(8); } + + /* AO_LOG_STATE elements */ + public int state() { return data16(0); } + public int reason() { return data16(2); } + + /* AO_LOG_SENSOR elements */ + public int pres() { return data32(0); } + public int temp() { return data32(4); } + public int accel() { return data16(8); } + + /* AO_LOG_TEMP_VOLT elements */ + public int v_batt() { return data16(0); } + public int sense_a() { return data16(2); } + public int sense_m() { return data16(4); } + + /* AO_LOG_GPS_POS elements */ + public int latitude() { return data32(0); } + public int longitude() { return data32(4); } + public int altitude() { return data16(8); } + + /* AO_LOG_GPS_TIME elements */ + public int hour() { return data8(0); } + public int minute() { return data8(1); } + public int second() { return data8(2); } + public int flags() { return data8(3); } + public int year() { return data8(4); } + public int month() { return data8(5); } + public int day() { return data8(6); } + + /* AO_LOG_GPS_SAT elements */ + public int nsat() { return data8(0); } + public int more() { return data8(1); } + public int svid(int n) { return data8(2 + n * 2); } + public int c_n(int n) { return data8(2 + n * 2 + 1); } + + public AltosEepromMetrum2 (AltosEepromChunk chunk, int start) throws ParseException { + parse_chunk(chunk, start); + } + + public void update_state(AltosState state) { + AltosGPS gps; + + /* Flush any pending GPS changes */ + if (state.gps_pending) { + switch (cmd) { + case AltosLib.AO_LOG_GPS_POS: + case AltosLib.AO_LOG_GPS_LAT: + case AltosLib.AO_LOG_GPS_LON: + case AltosLib.AO_LOG_GPS_ALT: + case AltosLib.AO_LOG_GPS_SAT: + case AltosLib.AO_LOG_GPS_DATE: + break; + default: + state.set_temp_gps(); + break; + } + } + + if (cmd != AltosLib.AO_LOG_FLIGHT) + state.set_tick(tick); + switch (cmd) { + case AltosLib.AO_LOG_FLIGHT: + state.set_boost_tick(tick); + state.set_flight(flight()); + state.set_ground_accel(ground_accel()); + state.set_ground_pressure(ground_pres()); +// state.set_temperature(ground_temp() / 100.0); + break; + case AltosLib.AO_LOG_STATE: + state.set_state(state()); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_ms5607(pres(), temp()); + state.set_accel(accel()); + + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt())); + + state.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense_a())); + state.set_main_voltage(AltosConvert.mega_pyro_voltage(sense_m())); + + break; + case AltosLib.AO_LOG_GPS_POS: + gps = state.make_temp_gps(); + gps.lat = latitude() / 1e7; + gps.lon = longitude() / 1e7; + gps.alt = altitude(); + break; + case AltosLib.AO_LOG_GPS_TIME: + gps = state.make_temp_gps(); + + gps.hour = hour(); + gps.minute = minute(); + gps.second = second(); + + int flags = flags(); + + gps.connected = (flags & AltosLib.AO_GPS_RUNNING) != 0; + gps.locked = (flags & AltosLib.AO_GPS_VALID) != 0; + gps.nsat = (flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> + AltosLib.AO_GPS_NUM_SAT_SHIFT; + + gps.year = year(); + gps.month = month(); + gps.day = day(); + break; + case AltosLib.AO_LOG_GPS_SAT: + state.set_tick(tick); + gps = state.make_temp_gps(); + + int n = nsat(); + for (int i = 0; i < n; i++) + gps.add_sat(svid(i), c_n(i)); + break; + } + } + + public AltosEepromMetrum2 (String line) { + parse_string(line); + } + + static public LinkedList read(FileInputStream input) { + LinkedList megas = new LinkedList(); + + for (;;) { + try { + String line = AltosLib.gets(input); + if (line == null) + break; + try { + AltosEepromMetrum2 mega = new AltosEepromMetrum2(line); + if (mega.cmd != AltosLib.AO_LOG_INVALID) + megas.add(mega); + } catch (Exception e) { + System.out.printf ("exception\n"); + } + } catch (IOException ie) { + break; + } + } + + return megas; + } +} diff --git a/altoslib/AltosEepromHeader.java b/altoslib/AltosEepromHeader.java index a06f05ed..35a03a12 100644 --- a/altoslib/AltosEepromHeader.java +++ b/altoslib/AltosEepromHeader.java @@ -29,6 +29,9 @@ public class AltosEepromHeader extends AltosEeprom { public boolean last; public boolean valid; + public int record_length () { return 0; } + + /* XXX pull rest of config data to state */ public void update_state(AltosState state) { switch (cmd) { case AltosLib.AO_LOG_CONFIG_VERSION: @@ -40,7 +43,7 @@ public class AltosEepromHeader extends AltosEeprom { case AltosLib.AO_LOG_RADIO_CHANNEL: break; case AltosLib.AO_LOG_CALLSIGN: - state.callsign = data; + state.set_callsign(data); break; case AltosLib.AO_LOG_ACCEL_CAL: state.set_accel_g(config_a, config_b); @@ -90,6 +93,7 @@ public class AltosEepromHeader extends AltosEeprom { state.baro.crc = config_a; break; case AltosLib.AO_LOG_SOFTWARE_VERSION: + state.set_firmware_version(data); break; } } diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java index 0804c392..e8f9b1fc 100644 --- a/altoslib/AltosEepromMega.java +++ b/altoslib/AltosEepromMega.java @@ -17,32 +17,14 @@ package org.altusmetrum.altoslib_1; +import java.io.*; +import java.util.*; import java.text.*; -public class AltosEepromMega { - public int cmd; - public int tick; - public boolean valid; - public String data; - public int config_a, config_b; - - public int data8[]; - +public class AltosEepromMega extends AltosEeprom { public static final int record_length = 32; - static final int header_length = 4; - static final int data_length = record_length - header_length; - - public int data8(int i) { - return data8[i]; - } - public int data16(int i) { - return ((data8[i] | (data8[i+1] << 8)) << 16) >> 16; - } - - public int data32(int i) { - return data8[i] | (data8[i+1] << 8) | (data8[i+2] << 16) | (data8[i+3] << 24); - } + public int record_length() { return record_length; } /* AO_LOG_FLIGHT elements */ public int flight() { return data16(0); } @@ -68,7 +50,7 @@ public class AltosEepromMega { public int mag_z() { return data16(24); } public int accel() { return data16(26); } - /* AO_LOG_VOLT elements */ + /* AO_LOG_TEMP_VOLT elements */ public int v_batt() { return data16(0); } public int v_pbatt() { return data16(2); } public int nsense() { return data16(4); } @@ -91,131 +73,137 @@ public class AltosEepromMega { public int nsat() { return data16(0); } public int svid(int n) { return data8(2 + n * 2); } public int c_n(int n) { return data8(2 + n * 2 + 1); } + public AltosEepromMega (AltosEepromChunk chunk, int start) throws ParseException { - cmd = chunk.data(start); - - valid = !chunk.erased(start, record_length); - if (valid) { - if (AltosConvert.checksum(chunk.data, start, record_length) != 0) - throw new ParseException(String.format("invalid checksum at 0x%x", - chunk.address + start), 0); - } else { - cmd = AltosLib.AO_LOG_INVALID; - } + parse_chunk(chunk, start); + } - tick = chunk.data16(start+2); + public void update_state(AltosState state) { + AltosGPS gps; + + /* Flush any pending GPS changes */ + if (state.gps_pending) { + switch (cmd) { + case AltosLib.AO_LOG_GPS_LAT: + case AltosLib.AO_LOG_GPS_LON: + case AltosLib.AO_LOG_GPS_ALT: + case AltosLib.AO_LOG_GPS_SAT: + case AltosLib.AO_LOG_GPS_DATE: + break; + default: + state.set_temp_gps(); + break; + } + } - data8 = new int[data_length]; - for (int i = 0; i < data_length; i++) - data8[i] = chunk.data(start + header_length + i); + switch (cmd) { + case AltosLib.AO_LOG_FLIGHT: + state.set_boost_tick(tick); + state.set_flight(flight()); + state.set_ground_accel(ground_accel()); + state.set_ground_pressure(ground_pres()); + state.set_temperature(ground_temp() / 100.0); + break; + case AltosLib.AO_LOG_STATE: + state.set_tick(tick); + state.set_state(state()); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_tick(tick); + state.set_ms5607(pres(), temp()); + + AltosIMU imu = new AltosIMU(); + imu.accel_x = accel_x(); + imu.accel_y = accel_y(); + imu.accel_z = accel_z(); + + imu.gyro_x = gyro_x(); + imu.gyro_y = gyro_y(); + imu.gyro_z = gyro_z(); + state.imu = imu; + + AltosMag mag = new AltosMag(); + mag.x = mag_x(); + mag.y = mag_y(); + mag.z = mag_z(); + + state.mag = mag; + + state.set_accel(accel()); + + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt())); + state.set_pyro_voltage(AltosConvert.mega_pyro_voltage(v_pbatt())); + + int nsense = nsense(); + + state.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense(nsense-2))); + state.set_main_voltage(AltosConvert.mega_pyro_voltage(sense(nsense-1))); + + double voltages[] = new double[nsense-2]; + for (int i = 0; i < nsense-2; i++) + voltages[i] = AltosConvert.mega_pyro_voltage(sense(i)); + + state.set_ignitor_voltage(voltages); + break; + case AltosLib.AO_LOG_GPS_TIME: + state.set_tick(tick); + gps = state.make_temp_gps(); + gps.lat = latitude() / 1e7; + gps.lon = longitude() / 1e7; + gps.alt = altitude(); + + gps.hour = hour(); + gps.minute = minute(); + gps.second = second(); + + int flags = flags(); + + gps.connected = (flags & AltosLib.AO_GPS_RUNNING) != 0; + gps.locked = (flags & AltosLib.AO_GPS_VALID) != 0; + gps.nsat = (flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> + AltosLib.AO_GPS_NUM_SAT_SHIFT; + + gps.year = year(); + gps.month = month(); + gps.day = day(); + break; + case AltosLib.AO_LOG_GPS_SAT: + state.set_tick(tick); + gps = state.make_temp_gps(); + + int n = nsat(); + for (int i = 0; i < n; i++) + gps.add_sat(svid(i), c_n(i)); + break; + } } public AltosEepromMega (String line) { - valid = false; - tick = 0; + parse_string(line); + } - if (line == null) { - cmd = AltosLib.AO_LOG_INVALID; - line = ""; - } else { + static public LinkedList read(FileInputStream input) { + LinkedList megas = new LinkedList(); + + for (;;) { try { - String[] tokens = line.split("\\s+"); - - if (tokens[0].length() == 1) { - if (tokens.length != 2 + data_length) { - cmd = AltosLib.AO_LOG_INVALID; - data = line; - } else { - cmd = tokens[0].codePointAt(0); - tick = Integer.parseInt(tokens[1],16); - valid = true; - data8 = new int[data_length]; - for (int i = 0; i < data_length; i++) - data8[i] = Integer.parseInt(tokens[2 + i],16); - } - } else if (tokens[0].equals("Config") && tokens[1].equals("version:")) { - cmd = AltosLib.AO_LOG_CONFIG_VERSION; - data = tokens[2]; - } else if (tokens[0].equals("Main") && tokens[1].equals("deploy:")) { - cmd = AltosLib.AO_LOG_MAIN_DEPLOY; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[0].equals("Apogee") && tokens[1].equals("delay:")) { - cmd = AltosLib.AO_LOG_APOGEE_DELAY; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[0].equals("Radio") && tokens[1].equals("channel:")) { - cmd = AltosLib.AO_LOG_RADIO_CHANNEL; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[0].equals("Callsign:")) { - cmd = AltosLib.AO_LOG_CALLSIGN; - data = tokens[1].replaceAll("\"",""); - } else if (tokens[0].equals("Accel") && tokens[1].equals("cal")) { - cmd = AltosLib.AO_LOG_ACCEL_CAL; - config_a = Integer.parseInt(tokens[3]); - config_b = Integer.parseInt(tokens[5]); - } else if (tokens[0].equals("Radio") && tokens[1].equals("cal:")) { - cmd = AltosLib.AO_LOG_RADIO_CAL; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[0].equals("Max") && tokens[1].equals("flight") && tokens[2].equals("log:")) { - cmd = AltosLib.AO_LOG_MAX_FLIGHT_LOG; - config_a = Integer.parseInt(tokens[3]); - } else if (tokens[0].equals("manufacturer")) { - cmd = AltosLib.AO_LOG_MANUFACTURER; - data = tokens[1]; - } else if (tokens[0].equals("product")) { - cmd = AltosLib.AO_LOG_PRODUCT; - data = tokens[1]; - } else if (tokens[0].equals("serial-number")) { - cmd = AltosLib.AO_LOG_SERIAL_NUMBER; - config_a = Integer.parseInt(tokens[1]); - } else if (tokens[0].equals("log-format")) { - cmd = AltosLib.AO_LOG_LOG_FORMAT; - config_a = Integer.parseInt(tokens[1]); - } else if (tokens[0].equals("software-version")) { - cmd = AltosLib.AO_LOG_SOFTWARE_VERSION; - data = tokens[1]; - } else if (tokens[0].equals("ms5607")) { - if (tokens[1].equals("reserved:")) { - cmd = AltosLib.AO_LOG_BARO_RESERVED; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("sens:")) { - cmd = AltosLib.AO_LOG_BARO_SENS; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("off:")) { - cmd = AltosLib.AO_LOG_BARO_OFF; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("tcs:")) { - cmd = AltosLib.AO_LOG_BARO_TCS; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("tco:")) { - cmd = AltosLib.AO_LOG_BARO_TCO; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("tref:")) { - cmd = AltosLib.AO_LOG_BARO_TREF; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("tempsens:")) { - cmd = AltosLib.AO_LOG_BARO_TEMPSENS; - config_a = Integer.parseInt(tokens[2]); - } else if (tokens[1].equals("crc:")) { - cmd = AltosLib.AO_LOG_BARO_CRC; - config_a = Integer.parseInt(tokens[2]); - } else { - cmd = AltosLib.AO_LOG_INVALID; - data = line; - } - } else { - cmd = AltosLib.AO_LOG_INVALID; - data = line; + String line = AltosLib.gets(input); + if (line == null) + break; + try { + AltosEepromMega mega = new AltosEepromMega(line); + if (mega.cmd != AltosLib.AO_LOG_INVALID) + megas.add(mega); + } catch (Exception e) { + System.out.printf ("exception\n"); } - } catch (NumberFormatException ne) { - cmd = AltosLib.AO_LOG_INVALID; - data = line; + } catch (IOException ie) { + break; } } - } - public AltosEepromMega(int in_cmd, int in_tick) { - cmd = in_cmd; - tick = in_tick; - valid = true; + return megas; } } diff --git a/altoslib/AltosEepromMetrum.java b/altoslib/AltosEepromMetrum.java index 72887032..e035e5fd 100644 --- a/altoslib/AltosEepromMetrum.java +++ b/altoslib/AltosEepromMetrum.java @@ -32,6 +32,8 @@ public class AltosEepromMetrum { static final int header_length = 4; static final int data_length = record_length - header_length; + public int record_length() { return record_length; } + public int data8(int i) { return data8[i]; } diff --git a/altoslib/AltosEepromMetrum2.java b/altoslib/AltosEepromMetrum2.java new file mode 100644 index 00000000..5a616e6c --- /dev/null +++ b/altoslib/AltosEepromMetrum2.java @@ -0,0 +1,178 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +import java.io.*; +import java.util.*; +import java.text.*; + +public class AltosEepromMetrum2 extends AltosEeprom { + public static final int record_length = 16; + + public int record_length() { return record_length; } + + /* AO_LOG_FLIGHT elements */ + public int flight() { return data16(0); } + public int ground_accel() { return data16(2); } + public int ground_pres() { return data32(4); } + public int ground_temp() { return data32(8); } + + /* AO_LOG_STATE elements */ + public int state() { return data16(0); } + public int reason() { return data16(2); } + + /* AO_LOG_SENSOR elements */ + public int pres() { return data32(0); } + public int temp() { return data32(4); } + public int accel() { return data16(8); } + + /* AO_LOG_TEMP_VOLT elements */ + public int v_batt() { return data16(0); } + public int sense_a() { return data16(2); } + public int sense_m() { return data16(4); } + + /* AO_LOG_GPS_POS elements */ + public int latitude() { return data32(0); } + public int longitude() { return data32(4); } + public int altitude() { return data16(8); } + + /* AO_LOG_GPS_TIME elements */ + public int hour() { return data8(0); } + public int minute() { return data8(1); } + public int second() { return data8(2); } + public int flags() { return data8(3); } + public int year() { return data8(4); } + public int month() { return data8(5); } + public int day() { return data8(6); } + + /* AO_LOG_GPS_SAT elements */ + public int nsat() { return data8(0); } + public int more() { return data8(1); } + public int svid(int n) { return data8(2 + n * 2); } + public int c_n(int n) { return data8(2 + n * 2 + 1); } + + public AltosEepromMetrum2 (AltosEepromChunk chunk, int start) throws ParseException { + parse_chunk(chunk, start); + } + + public void update_state(AltosState state) { + AltosGPS gps; + + /* Flush any pending GPS changes */ + if (state.gps_pending) { + switch (cmd) { + case AltosLib.AO_LOG_GPS_POS: + case AltosLib.AO_LOG_GPS_LAT: + case AltosLib.AO_LOG_GPS_LON: + case AltosLib.AO_LOG_GPS_ALT: + case AltosLib.AO_LOG_GPS_SAT: + case AltosLib.AO_LOG_GPS_DATE: + break; + default: + state.set_temp_gps(); + break; + } + } + + if (cmd != AltosLib.AO_LOG_FLIGHT) + state.set_tick(tick); + switch (cmd) { + case AltosLib.AO_LOG_FLIGHT: + state.set_boost_tick(tick); + state.set_flight(flight()); + state.set_ground_accel(ground_accel()); + state.set_ground_pressure(ground_pres()); +// state.set_temperature(ground_temp() / 100.0); + break; + case AltosLib.AO_LOG_STATE: + state.set_state(state()); + break; + case AltosLib.AO_LOG_SENSOR: + state.set_ms5607(pres(), temp()); + state.set_accel(accel()); + + break; + case AltosLib.AO_LOG_TEMP_VOLT: + state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt())); + + state.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense_a())); + state.set_main_voltage(AltosConvert.mega_pyro_voltage(sense_m())); + + break; + case AltosLib.AO_LOG_GPS_POS: + gps = state.make_temp_gps(); + gps.lat = latitude() / 1e7; + gps.lon = longitude() / 1e7; + gps.alt = altitude(); + break; + case AltosLib.AO_LOG_GPS_TIME: + gps = state.make_temp_gps(); + + gps.hour = hour(); + gps.minute = minute(); + gps.second = second(); + + int flags = flags(); + + gps.connected = (flags & AltosLib.AO_GPS_RUNNING) != 0; + gps.locked = (flags & AltosLib.AO_GPS_VALID) != 0; + gps.nsat = (flags & AltosLib.AO_GPS_NUM_SAT_MASK) >> + AltosLib.AO_GPS_NUM_SAT_SHIFT; + + gps.year = year(); + gps.month = month(); + gps.day = day(); + break; + case AltosLib.AO_LOG_GPS_SAT: + state.set_tick(tick); + gps = state.make_temp_gps(); + + int n = nsat(); + for (int i = 0; i < n; i++) + gps.add_sat(svid(i), c_n(i)); + break; + } + } + + public AltosEepromMetrum2 (String line) { + parse_string(line); + } + + static public LinkedList read(FileInputStream input) { + LinkedList megas = new LinkedList(); + + for (;;) { + try { + String line = AltosLib.gets(input); + if (line == null) + break; + try { + AltosEepromMetrum2 mega = new AltosEepromMetrum2(line); + if (mega.cmd != AltosLib.AO_LOG_INVALID) + megas.add(mega); + } catch (Exception e) { + System.out.printf ("exception\n"); + } + } catch (IOException ie) { + break; + } + } + + return megas; + } +} diff --git a/altoslib/AltosEepromMini.java b/altoslib/AltosEepromMini.java index 1e0ff1b9..15ec1929 100644 --- a/altoslib/AltosEepromMini.java +++ b/altoslib/AltosEepromMini.java @@ -24,21 +24,7 @@ import java.text.*; public class AltosEepromMini extends AltosEeprom { public static final int record_length = 16; - public int data8(int i) { - return data8[i]; - } - - public int data16(int i) { - return ((data8[i] | (data8[i+1] << 8)) << 16) >> 16; - } - - public int data24(int i) { - return data8[i] | (data8[i+1] << 8) | (data8[i+2] << 16); - } - - public int data32(int i) { - return data8[i] | (data8[i+1] << 8) | (data8[i+2] << 16) | (data8[i+3] << 24); - } + public int record_length() { return record_length; } /* AO_LOG_FLIGHT elements */ public int flight() { return data16(0); } @@ -84,11 +70,11 @@ public class AltosEepromMini extends AltosEeprom { } public AltosEepromMini (AltosEepromChunk chunk, int start) throws ParseException { - parse_chunk(chunk, start, record_length); + parse_chunk(chunk, start); } public AltosEepromMini (String line) { - parse_string(line, record_length); + parse_string(line); } public AltosEepromMini(int in_cmd, int in_tick) { diff --git a/altoslib/AltosEepromTM.java b/altoslib/AltosEepromTM.java index 6945468b..461a7a9c 100644 --- a/altoslib/AltosEepromTM.java +++ b/altoslib/AltosEepromTM.java @@ -30,16 +30,16 @@ public class AltosEepromTM extends AltosEeprom { public static final int record_length = 8; - static double - thermometer_to_temperature(double thermo) - { - return (thermo - 19791.268) / 32728.0 * 1.25 / 0.00247; - } - public void write(PrintStream out) { out.printf("%c %4x %4x %4x\n", cmd, tick, a, b); } + public int record_length() { return record_length; } + + public String string() { + return String.format("%c %4x %4x %4x\n", cmd, tick, a, b); + } + public void update_state(AltosState state) { AltosGPS gps; @@ -77,7 +77,7 @@ public class AltosEepromTM extends AltosEeprom { break; case AltosLib.AO_LOG_TEMP_VOLT: state.set_tick(tick); - state.set_temperature(thermometer_to_temperature(a)); + state.set_temperature(AltosConvert.thermometer_to_temperature(a)); state.set_battery_voltage(AltosConvert.cc_battery_to_voltage(b)); break; case AltosLib.AO_LOG_DEPLOY: diff --git a/altoslib/AltosEepromTeleScience.java b/altoslib/AltosEepromTeleScience.java index 2a828cf3..bacd66b5 100644 --- a/altoslib/AltosEepromTeleScience.java +++ b/altoslib/AltosEepromTeleScience.java @@ -33,6 +33,8 @@ public class AltosEepromTeleScience { static final int max_data = 12; public static final int record_length = 32; + public int record_length() { return record_length; } + public AltosEepromTeleScience (AltosEepromChunk chunk, int start) throws ParseException { type = chunk.data(start); diff --git a/altoslib/AltosGPS.java b/altoslib/AltosGPS.java index 399e95b1..a8c19e4a 100644 --- a/altoslib/AltosGPS.java +++ b/altoslib/AltosGPS.java @@ -65,8 +65,8 @@ public class AltosGPS implements Cloneable { } public void ClearGPSTime() { - year = month = day = 0; - hour = minute = second = 0; + year = month = day = AltosRecord.MISSING; + hour = minute = second = AltosRecord.MISSING; } public AltosGPS(AltosTelemetryMap map) throws ParseException { @@ -212,6 +212,9 @@ public class AltosGPS implements Cloneable { } public AltosGPS() { + lat = AltosRecord.MISSING; + lon = AltosRecord.MISSING; + alt = AltosRecord.MISSING; ClearGPSTime(); cc_gps_sat = null; } @@ -280,6 +283,9 @@ public class AltosGPS implements Cloneable { } } } else { + lat = AltosRecord.MISSING; + lon = AltosRecord.MISSING; + alt = AltosRecord.MISSING; ClearGPSTime(); cc_gps_sat = null; } diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index 4ca8ad9d..d6d78ca8 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -28,6 +28,7 @@ public class AltosLib { public static final int AO_LOG_TEMP_VOLT = 'T'; public static final int AO_LOG_DEPLOY = 'D'; public static final int AO_LOG_STATE = 'S'; + public static final int AO_LOG_GPS_POS = 'P'; public static final int AO_LOG_GPS_TIME = 'G'; public static final int AO_LOG_GPS_LAT = 'N'; public static final int AO_LOG_GPS_LON = 'W'; diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index aa3de432..b80d7b2e 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -32,7 +32,7 @@ public class AltosState implements Cloneable { /* derived data */ - public long report_time; + public long received_time; public double time; public double prev_time; @@ -48,6 +48,12 @@ public class AltosState implements Cloneable { public boolean boost; /* under power */ public int rssi; public int status; + public int device_type; + public int config_major; + public int config_minor; + public int apogee_delay; + public int main_deploy; + public int flight_log_max; public double ground_altitude; public double ground_pressure; @@ -61,11 +67,16 @@ public class AltosState implements Cloneable { public double apogee_voltage; public double main_voltage; public double speed; + public double ignitor_voltage[]; public double prev_height; public double prev_speed; public double prev_acceleration; + public double prev_max_height; + public double prev_max_acceleration; + public double prev_max_speed; + public double max_height; public double max_acceleration; public double max_speed; @@ -100,6 +111,8 @@ public class AltosState implements Cloneable { public double speak_altitude; public String callsign; + public String firmware_version; + public double accel_plus_g; public double accel_minus_g; public double accel; @@ -133,7 +146,7 @@ public class AltosState implements Cloneable { set = 0; - report_time = System.currentTimeMillis(); + received_time = System.currentTimeMillis(); time = AltosRecord.MISSING; time_change = AltosRecord.MISSING; prev_time = AltosRecord.MISSING; @@ -145,6 +158,12 @@ public class AltosState implements Cloneable { boost = false; rssi = AltosRecord.MISSING; status = 0; + device_type = AltosRecord.MISSING; + config_major = AltosRecord.MISSING; + config_minor = AltosRecord.MISSING; + apogee_delay = AltosRecord.MISSING; + main_deploy = AltosRecord.MISSING; + flight_log_max = AltosRecord.MISSING; ground_altitude = AltosRecord.MISSING; ground_pressure = AltosRecord.MISSING; @@ -158,10 +177,15 @@ public class AltosState implements Cloneable { prev_speed = AltosRecord.MISSING; prev_acceleration = AltosRecord.MISSING; + prev_max_height = 0; + prev_max_speed = 0; + prev_max_acceleration = 0; + battery_voltage = AltosRecord.MISSING; pyro_voltage = AltosRecord.MISSING; apogee_voltage = AltosRecord.MISSING; main_voltage = AltosRecord.MISSING; + ignitor_voltage = null; speed = AltosRecord.MISSING; @@ -219,7 +243,7 @@ public class AltosState implements Cloneable { return; } - report_time = old.report_time; + received_time = old.received_time; time = old.time; time_change = 0; tick = old.tick; @@ -232,6 +256,12 @@ public class AltosState implements Cloneable { boost = old.boost; rssi = old.rssi; status = old.status; + device_type = old.device_type; + config_major = old.config_major; + config_minor = old.config_minor; + apogee_delay = old.apogee_delay; + main_deploy = old.main_deploy; + flight_log_max = old.flight_log_max; set = 0; @@ -245,11 +275,16 @@ public class AltosState implements Cloneable { temperature = old.temperature; apogee_voltage = old.apogee_voltage; main_voltage = old.main_voltage; + ignitor_voltage = old.ignitor_voltage; speed = old.speed; prev_height = old.height; prev_speed = old.speed; prev_acceleration = old.acceleration; + + prev_max_height = old.max_height; + prev_max_speed = old.max_speed; + prev_max_acceleration = old.max_acceleration; prev_time = old.time; max_height = old.max_height; @@ -343,7 +378,7 @@ public class AltosState implements Cloneable { else height = AltosRecord.MISSING; - if (height != AltosRecord.MISSING && height > max_height) + if (height != AltosRecord.MISSING && height > prev_max_height) max_height = height; update_speed(); @@ -394,31 +429,34 @@ public class AltosState implements Cloneable { } } } - if (boost && speed != AltosRecord.MISSING && speed > max_speed) + if (boost && speed != AltosRecord.MISSING && speed > prev_max_speed) max_speed = speed; } void update_accel() { - double ground = ground_accel; - - if (ground == AltosRecord.MISSING) - ground = ground_accel_avg; - if (accel == AltosRecord.MISSING) - return; - if (ground == AltosRecord.MISSING) - return; - if (accel_plus_g == AltosRecord.MISSING) - return; - if (accel_minus_g == AltosRecord.MISSING) - return; - - double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; - double counts_per_mss = counts_per_g / 9.80665; - - acceleration = (ground - accel) / counts_per_mss; + if (kalman_acceleration != AltosRecord.MISSING) { + acceleration = kalman_acceleration; + } else { + double ground = ground_accel; + + if (ground == AltosRecord.MISSING) + ground = ground_accel_avg; + if (accel == AltosRecord.MISSING) + return; + if (ground == AltosRecord.MISSING) + return; + if (accel_plus_g == AltosRecord.MISSING) + return; + if (accel_minus_g == AltosRecord.MISSING) + return; + + double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; + double counts_per_mss = counts_per_g / 9.80665; + acceleration = (ground - accel) / counts_per_mss; + } /* Only look at accelerometer data under boost */ - if (boost && acceleration != AltosRecord.MISSING && (max_acceleration == AltosRecord.MISSING || acceleration > max_acceleration)) + if (boost && acceleration != AltosRecord.MISSING && acceleration > prev_max_acceleration) max_acceleration = acceleration; update_speed(); } @@ -502,6 +540,26 @@ public class AltosState implements Cloneable { } + public void set_device_type(int device_type) { + this.device_type = device_type; + } + + public void set_config(int major, int minor, int apogee_delay, int main_deploy, int flight_log_max) { + config_major = major; + config_minor = minor; + this.apogee_delay = apogee_delay; + this.main_deploy = main_deploy; + this.flight_log_max = flight_log_max; + } + + public void set_callsign(String callsign) { + this.callsign = callsign; + } + + public void set_firmware_version(String version) { + firmware_version = version; + } + public void set_flight(int flight) { /* When the flight changes, reset the state */ @@ -538,6 +596,10 @@ public class AltosState implements Cloneable { } } + public void set_received_time(long ms) { + received_time = ms; + } + public void set_altitude(double altitude) { if (altitude != AltosRecord.MISSING) { this.altitude = altitude; @@ -577,6 +639,8 @@ public class AltosState implements Cloneable { kalman_speed = speed; kalman_acceleration = acceleration; update_vertical_pos(); + update_speed(); + update_accel(); } } @@ -672,6 +736,9 @@ public class AltosState implements Cloneable { } } + public void set_ignitor_voltage(double[] voltage) { + this.ignitor_voltage = voltage; + } public double time_since_boost() { if (tick == AltosRecord.MISSING) @@ -702,6 +769,13 @@ public class AltosState implements Cloneable { temp_gps = null; } + public AltosState clone() { + AltosState s = new AltosState(); + s.copy(this); + return s; + } + + public void init (AltosRecord cur, AltosState prev_state) { System.out.printf ("init\n"); @@ -721,7 +795,7 @@ public class AltosState implements Cloneable { set_kalman(cur.kalman_height, cur.kalman_speed, cur.kalman_acceleration); - report_time = System.currentTimeMillis(); + received_time = System.currentTimeMillis(); set_temperature(cur.temperature()); set_apogee_voltage(cur.drogue_voltage()); @@ -742,12 +816,6 @@ public class AltosState implements Cloneable { } - public AltosState clone() { - AltosState s = new AltosState(); - s.copy(this); - return s; - } - public AltosState(AltosRecord cur) { init(cur, null); } @@ -756,6 +824,7 @@ public class AltosState implements Cloneable { init(cur, prev); } + public AltosState () { init(); } diff --git a/altoslib/AltosTelemetry.java b/altoslib/AltosTelemetry.java index b84455d3..82e5400e 100644 --- a/altoslib/AltosTelemetry.java +++ b/altoslib/AltosTelemetry.java @@ -43,6 +43,10 @@ public abstract class AltosTelemetry implements AltosStateUpdate { } public void update_state(AltosState state) { + state.set_serial(serial); + state.set_tick(tick); + state.set_rssi(rssi, status); + state.set_received_time(received_time); } final static int PKT_APPEND_STATUS_1_CRC_OK = (1 << 7); @@ -56,9 +60,11 @@ public abstract class AltosTelemetry implements AltosStateUpdate { final static int packet_type_location = 0x05; final static int packet_type_satellite = 0x06; final static int packet_type_companion = 0x07; - final static int packet_type_MM_sensor = 0x08; - final static int packet_type_MM_data = 0x09; - final static int packet_type_Mini = 0x10; + final static int packet_type_mega_sensor = 0x08; + final static int packet_type_mega_data = 0x09; + final static int packet_type_metrum_sensor = 0x0a; + final static int packet_type_metrum_data = 0x0b; + final static int packet_type_mini = 0x10; static AltosTelemetry parse_hex(String hex) throws ParseException, AltosCRCException { AltosTelemetry telem = null; @@ -87,37 +93,7 @@ public abstract class AltosTelemetry implements AltosStateUpdate { /* length, data ..., rssi, status, checksum -- 4 bytes extra */ switch (bytes.length) { case AltosLib.ao_telemetry_standard_len + 4: - int type = AltosLib.uint8(bytes, 4 + 1); -/* - switch (type) { - case packet_type_TM_sensor: - case packet_type_Tm_sensor: - case packet_type_Tn_sensor: - telem = new AltosTelemetrySensor(bytes); - break; - case packet_type_configuration: - telem = new AltosTelemetryConfiguration(bytes); - break; - case packet_type_location: - telem = new AltosTelemetryLocation(bytes); - break; - case packet_type_satellite: - telem = new AltosTelemetrySatellite(bytes); - break; - case packet_type_companion: - telem = new AltosTelemetryCompanion(bytes); - break; - case packet_type_MM_sensor: - telem = new AltosTelemetryMegaSensor(bytes); - break; - case packet_type_MM_data: - telem = new AltosTelemetryMegaData(bytes); - break; - default: - telem = new AltosTelemetryRaw(bytes); - break; - } -*/ + telem = AltosTelemetryStandard.parse_hex(bytes); break; case AltosLib.ao_telemetry_0_9_len + 4: telem = new AltosTelemetryLegacy(bytes); diff --git a/altoslib/AltosTelemetryConfiguration.java b/altoslib/AltosTelemetryConfiguration.java new file mode 100644 index 00000000..4c9bdd1f --- /dev/null +++ b/altoslib/AltosTelemetryConfiguration.java @@ -0,0 +1,55 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + + +public class AltosTelemetryConfiguration extends AltosTelemetryStandard { + int device_type; + int flight; + int config_major; + int config_minor; + int apogee_delay; + int main_deploy; + int flight_log_max; + String callsign; + String version; + + public AltosTelemetryConfiguration(int[] bytes) { + super(bytes); + + device_type = uint8(5); + flight = uint16(6); + config_major = uint8(8); + config_minor = uint8(9); + apogee_delay = uint16(10); + main_deploy = uint16(12); + flight_log_max = uint16(14); + callsign = string(16, 8); + version = string(24, 8); + } + + public void update_state(AltosState state) { + super.update_state(state); + state.set_device_type(device_type); + state.set_flight(flight); + state.set_config(config_major, config_minor, apogee_delay, main_deploy, flight_log_max); + + state.set_callsign(callsign); + state.set_firmware_version(version); + } +} diff --git a/altoslib/AltosTelemetryLegacy.java b/altoslib/AltosTelemetryLegacy.java index 45e5c315..95cbbeed 100644 --- a/altoslib/AltosTelemetryLegacy.java +++ b/altoslib/AltosTelemetryLegacy.java @@ -546,7 +546,7 @@ public class AltosTelemetryLegacy extends AltosTelemetry { state.set_accel(accel); if (kalman_height != AltosRecord.MISSING) state.set_kalman(kalman_height, kalman_speed, kalman_acceleration); - state.set_temperature(AltosEepromTM.thermometer_to_temperature(temp)); + state.set_temperature(AltosConvert.thermometer_to_temperature(temp)); state.set_battery_voltage(AltosConvert.cc_battery_to_voltage(batt)); state.set_apogee_voltage(AltosConvert.cc_ignitor_to_voltage(apogee)); state.set_main_voltage(AltosConvert.cc_ignitor_to_voltage(main)); diff --git a/altoslib/AltosTelemetryLocation.java b/altoslib/AltosTelemetryLocation.java new file mode 100644 index 00000000..fa3c24d0 --- /dev/null +++ b/altoslib/AltosTelemetryLocation.java @@ -0,0 +1,88 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + + +public class AltosTelemetryLocation extends AltosTelemetryStandard { + int flags; + int altitude; + int latitude; + int longitude; + int year; + int month; + int day; + int hour; + int minute; + int second; + int pdop; + int hdop; + int vdop; + int mode; + int ground_speed; + int climb_rate; + int course; + + public AltosTelemetryLocation(int[] bytes) { + super(bytes); + + flags = uint8(5); + altitude = int16(6); + latitude = uint32(8); + longitude = uint32(12); + year = uint8(16); + month = uint8(17); + day = uint8(18); + hour = uint8(19); + minute = uint8(20); + second = uint8(21); + pdop = uint8(22); + hdop = uint8(23); + vdop = uint8(24); + mode = uint8(25); + ground_speed = uint16(26); + climb_rate = int16(28); + course = uint8(30); + } + + public void update_state(AltosState state) { + super.update_state(state); + AltosGPS gps = state.make_temp_gps(); + + gps.nsat = flags & 0xf; + gps.locked = (flags & (1 << 4)) != 0; + gps.connected = (flags & (1 << 5)) != 0; + + if (gps.locked) { + gps.lat = latitude * 1.0e-7; + gps.lon = longitude * 1.0e-7; + gps.alt = altitude; + gps.year = 2000 + year; + gps.month = month; + gps.day = day; + gps.hour = hour; + gps.minute = minute; + gps.second = second; + gps.ground_speed = ground_speed * 1.0e-2; + gps.course = course * 2; + gps.climb_rate = climb_rate * 1.0e-2; + gps.hdop = hdop; + gps.vdop = vdop; + } + state.set_temp_gps(); + } +} diff --git a/altoslib/AltosTelemetryMegaData.java b/altoslib/AltosTelemetryMegaData.java new file mode 100644 index 00000000..5e6cd580 --- /dev/null +++ b/altoslib/AltosTelemetryMegaData.java @@ -0,0 +1,85 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +public class AltosTelemetryMegaData extends AltosTelemetryStandard { + int state; + + int v_batt; + int v_pyro; + int sense[]; + + int ground_pres; + int ground_accel; + int accel_plus_g; + int accel_minus_g; + + int acceleration; + int speed; + int height; + + public AltosTelemetryMegaData(int[] bytes) { + super(bytes); + + state = int8(5); + + v_batt = int16(6); + v_pyro = int16(8); + + sense = new int[6]; + + for (int i = 0; i < 6; i++) { + sense[i] = int8(10 + i) << 4; + sense[i] |= sense[i] >> 8; + } + + ground_pres = int32(16); + ground_accel = int16(20); + accel_plus_g = int16(22); + accel_minus_g = int16(24); + + acceleration = int16(26); + speed = int16(28); + height = int16(30); + } + + public void update_state(AltosState state) { + super.update_state(state); + + state.set_state(this.state); + + state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt)); + state.set_pyro_voltage(AltosConvert.mega_pyro_voltage(v_pyro)); + + state.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense[4])); + state.set_main_voltage(AltosConvert.mega_pyro_voltage(sense[5])); + + double voltages[] = new double[4]; + for (int i = 0; i < 4; i++) + voltages[i] = AltosConvert.mega_pyro_voltage(sense[i]); + + state.set_ignitor_voltage(voltages); + + state.set_ground_accel(ground_accel); + state.set_ground_pressure(ground_pres); + state.set_accel_g(accel_plus_g, accel_minus_g); + + state.set_kalman(height, speed/16.0, acceleration / 16.0); + } +} + diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java new file mode 100644 index 00000000..7c385cfd --- /dev/null +++ b/altoslib/AltosTelemetryMegaSensor.java @@ -0,0 +1,84 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +public class AltosTelemetryMegaSensor extends AltosTelemetryStandard { + int accel; + int pres; + int temp; + + int accel_x; + int accel_y; + int accel_z; + + int gyro_x; + int gyro_y; + int gyro_z; + + int mag_x; + int mag_y; + int mag_z; + + public AltosTelemetryMegaSensor(int[] bytes) { + super(bytes); + + accel = int16(6); + pres = int32(8); + temp = int16(12); + + accel_x = int16(14); + accel_y = int16(16); + accel_z = int16(18); + + gyro_x = int16(20); + gyro_y = int16(22); + gyro_z = int16(24); + + mag_x = int16(26); + mag_y = int16(28); + mag_z = int16(30); + } + + public void update_state(AltosState state) { + super.update_state(state); + + state.set_accel(accel); + state.set_pressure(pres); + state.set_temperature(temp / 100.0); + + AltosIMU imu = new AltosIMU(); + + imu.accel_x = accel_x; + imu.accel_y = accel_y; + imu.accel_z = accel_z; + + imu.gyro_x = gyro_x; + imu.gyro_y = gyro_y; + imu.gyro_z = gyro_z; + + state.imu = imu; + + AltosMag mag = new AltosMag(); + + mag.x = mag_x; + mag.y = mag_y; + mag.z = mag_z; + + state.mag = mag; + } +} diff --git a/altoslib/AltosTelemetryMetrumData.java b/altoslib/AltosTelemetryMetrumData.java new file mode 100644 index 00000000..d419ab80 --- /dev/null +++ b/altoslib/AltosTelemetryMetrumData.java @@ -0,0 +1,42 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + + +public class AltosTelemetryMetrumData extends AltosTelemetryStandard { + + int ground_pres; + int ground_accel; + int accel_plus_g; + int accel_minus_g; + + public AltosTelemetryMetrumData(int[] bytes) { + super(bytes); + + ground_pres = int32(8); + ground_accel = int16(12); + accel_plus_g = int16(14); + accel_minus_g = int16(16); + } + + public void update_state(AltosState state) { + state.set_ground_accel(ground_accel); + state.set_accel_g(accel_plus_g, accel_minus_g); + state.set_ground_pressure(ground_pres); + } +} diff --git a/altoslib/AltosTelemetryMetrumSensor.java b/altoslib/AltosTelemetryMetrumSensor.java new file mode 100644 index 00000000..59d34dba --- /dev/null +++ b/altoslib/AltosTelemetryMetrumSensor.java @@ -0,0 +1,69 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + + +public class AltosTelemetryMetrumSensor extends AltosTelemetryStandard { + int state; + + int accel; + int pres; + int temp; + + int acceleration; + int speed; + int height; + + int v_batt; + int sense_a; + int sense_m; + + public AltosTelemetryMetrumSensor(int[] bytes) { + super(bytes); + + state = int8(5); + accel = int16(6); + pres = int32(8); + temp = int16(12); + + acceleration = int16(14); + speed = int16(16); + height = int16(18); + + v_batt = int16(20); + sense_a = int16(22); + sense_m = int16(24); + } + + public void update_state(AltosState state) { + super.update_state(state); + + state.set_state(this.state); + + state.set_accel(accel); + state.set_ms5607(pres, temp); + + state.set_kalman(height, speed/16.0, acceleration/16.0); + + state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt)); + + state.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense_a)); + state.set_main_voltage(AltosConvert.mega_pyro_voltage(sense_m)); + System.out.printf ("sense_a %d apogee voltage %g\n", sense_a, state.apogee_voltage); + } +} diff --git a/altoslib/AltosTelemetryRaw.java b/altoslib/AltosTelemetryRaw.java new file mode 100644 index 00000000..9ef7787e --- /dev/null +++ b/altoslib/AltosTelemetryRaw.java @@ -0,0 +1,28 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +public class AltosTelemetryRaw extends AltosTelemetryStandard { + public AltosTelemetryRaw(int[] bytes) { + super(bytes); + } + + public void update_state(AltosState state) { + super.update_state(state); + } +} diff --git a/altoslib/AltosTelemetrySatellite.java b/altoslib/AltosTelemetrySatellite.java new file mode 100644 index 00000000..3f70f212 --- /dev/null +++ b/altoslib/AltosTelemetrySatellite.java @@ -0,0 +1,50 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +public class AltosTelemetrySatellite extends AltosTelemetryStandard { + int channels; + AltosGPSSat[] sats; + + public AltosTelemetrySatellite(int[] bytes) { + super(bytes); + + channels = uint8(5); + if (channels > 12) + channels = 12; + if (channels == 0) + sats = null; + else { + sats = new AltosGPSSat[channels]; + for (int i = 0; i < channels; i++) { + int svid = uint8(6 + i * 2 + 0); + int c_n_1 = uint8(6 + i * 2 + 1); + sats[i] = new AltosGPSSat(svid, c_n_1); + } + } + } + + public void update_state(AltosState state) { + super.update_state(state); + + AltosGPS gps = state.make_temp_gps(); + + gps.cc_gps_sat = sats; + state.set_temp_gps(); + } +} diff --git a/altoslib/AltosTelemetrySensor.java b/altoslib/AltosTelemetrySensor.java new file mode 100644 index 00000000..f89e56c3 --- /dev/null +++ b/altoslib/AltosTelemetrySensor.java @@ -0,0 +1,80 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + + +public class AltosTelemetrySensor extends AltosTelemetryStandard { + int state; + int accel; + int pres; + int temp; + int v_batt; + int sense_d; + int sense_m; + + int acceleration; + int speed; + int height; + + int ground_accel; + int ground_pres; + int accel_plus_g; + int accel_minus_g; + + public AltosTelemetrySensor(int[] bytes) { + super(bytes); + state = uint8(5); + + accel = int16(6); + pres = int16(8); + temp = int16(10); + v_batt = int16(12); + sense_d = int16(14); + sense_m = int16(16); + + acceleration = int16(18); + speed = int16(20); + height = int16(22); + + ground_pres = int16(24); + ground_accel = int16(26); + accel_plus_g = int16(28); + accel_minus_g = int16(30); + } + + public void update_state(AltosState state) { + super.update_state(state); + + state.set_state(this.state); + if (type == packet_type_TM_sensor) { + state.set_ground_accel(ground_accel); + state.set_accel_g(accel_plus_g, accel_minus_g); + state.set_accel(accel); + } + state.set_ground_pressure(AltosConvert.barometer_to_pressure(ground_pres)); + state.set_pressure(AltosConvert.barometer_to_pressure(pres)); + state.set_temperature(AltosConvert.thermometer_to_temperature(temp)); + state.set_battery_voltage(AltosConvert.cc_battery_to_voltage(v_batt)); + if (type == packet_type_TM_sensor || type == packet_type_Tm_sensor) { + state.set_apogee_voltage(AltosConvert.cc_ignitor_to_voltage(sense_d)); + state.set_main_voltage(AltosConvert.cc_ignitor_to_voltage(sense_m)); + } + + state.set_kalman(height, speed/16.0, acceleration / 16.0); + } +} diff --git a/altoslib/AltosTelemetryStandard.java b/altoslib/AltosTelemetryStandard.java new file mode 100644 index 00000000..fa86bf8e --- /dev/null +++ b/altoslib/AltosTelemetryStandard.java @@ -0,0 +1,106 @@ +/* + * 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. + */ + +package org.altusmetrum.altoslib_1; + +public abstract class AltosTelemetryStandard extends AltosTelemetry { + int[] bytes; + int type; + + public int int8(int off) { + return AltosLib.int8(bytes, off + 1); + } + + public int uint8(int off) { + return AltosLib.uint8(bytes, off + 1); + } + + public int int16(int off) { + return AltosLib.int16(bytes, off + 1); + } + + public int uint16(int off) { + return AltosLib.uint16(bytes, off + 1); + } + + public int uint32(int off) { + return AltosLib.uint32(bytes, off + 1); + } + + public int int32(int off) { + return AltosLib.int32(bytes, off + 1); + } + + public String string(int off, int l) { + return AltosLib.string(bytes, off + 1, l); + } + + public static AltosTelemetry parse_hex(int[] bytes) { + int type = AltosLib.uint8(bytes, 4 + 1); + + AltosTelemetry telem; + switch (type) { + case packet_type_TM_sensor: + case packet_type_Tm_sensor: + case packet_type_Tn_sensor: + telem = new AltosTelemetrySensor(bytes); + break; + case packet_type_configuration: + telem = new AltosTelemetryConfiguration(bytes); + break; + case packet_type_location: + telem = new AltosTelemetryLocation(bytes); + break; + case packet_type_satellite: + telem = new AltosTelemetrySatellite(bytes); + break; +/* + case packet_type_companion: + telem = new AltosTelemetryCompanion(bytes); + break; +*/ + case packet_type_mega_sensor: + telem = new AltosTelemetryMegaSensor(bytes); + break; + case packet_type_mega_data: + telem = new AltosTelemetryMegaData(bytes); + break; + case packet_type_metrum_sensor: + telem = new AltosTelemetryMetrumSensor(bytes); + break; + case packet_type_metrum_data: + telem = new AltosTelemetryMetrumData(bytes); + break; + default: + telem = new AltosTelemetryRaw(bytes); + break; + } + return telem; + } + + public AltosTelemetryStandard(int[] bytes) { + this.bytes = bytes; + + serial = uint16(0); + tick = uint16(2); + type = uint8(4); + } + + public void update_state(AltosState state) { + super.update_state(state); + } +} diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 59e0ec1c..87d4d898 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -10,7 +10,19 @@ SRC=. altoslibdir = $(datadir)/java +record_files = \ + AltosEepromRecord.java \ + AltosEepromTeleScience.java \ + AltosRecordCompanion.java \ + AltosRecordIterable.java \ + AltosRecord.java \ + AltosRecordNone.java \ + AltosRecordTM.java \ + AltosRecordMM.java \ + AltosRecordMini.java + altoslib_JAVA = \ + $(record_files) \ AltosLib.java \ AltosConfigData.java \ AltosConfigValues.java \ @@ -25,11 +37,8 @@ altoslib_JAVA = \ AltosEepromIterable.java \ AltosEepromLog.java \ AltosEepromMega.java \ - AltosEepromMegaIterable.java \ - AltosEepromRecord.java \ - AltosEepromTeleScience.java \ + AltosEepromMetrum2.java \ AltosEepromMini.java \ - AltosEepromOldIterable.java \ AltosFile.java \ AltosFlash.java \ AltosFlashListener.java \ @@ -57,13 +66,6 @@ altoslib_JAVA = \ AltosParse.java \ AltosPreferences.java \ AltosPreferencesBackend.java \ - AltosRecordCompanion.java \ - AltosRecordIterable.java \ - AltosRecord.java \ - AltosRecordNone.java \ - AltosRecordTM.java \ - AltosRecordMM.java \ - AltosRecordMini.java \ AltosReplayReader.java \ AltosRomconfig.java \ AltosSensorMM.java \ @@ -72,11 +74,21 @@ altoslib_JAVA = \ AltosStateIterable.java \ AltosStateUpdate.java \ AltosTelemetry.java \ + AltosTelemetryConfiguration.java \ AltosTelemetryFile.java \ AltosTelemetryIterable.java \ AltosTelemetryLegacy.java \ + AltosTelemetryLocation.java \ AltosTelemetryMap.java \ + AltosTelemetryMegaSensor.java \ + AltosTelemetryMegaData.java \ + AltosTelemetryMetrumSensor.java \ + AltosTelemetryMetrumData.java \ AltosTelemetryReader.java \ + AltosTelemetryRaw.java \ + AltosTelemetrySensor.java \ + AltosTelemetrySatellite.java \ + AltosTelemetryStandard.java \ AltosUnitsListener.java \ AltosMs5607.java \ AltosIMU.java \ diff --git a/altosui/AltosAscent.java b/altosui/AltosAscent.java index f8435037..ceba2d1d 100644 --- a/altosui/AltosAscent.java +++ b/altosui/AltosAscent.java @@ -308,7 +308,7 @@ public class AltosAscent extends JComponent implements AltosFlightDisplay { class Lat extends AscentValue { void show (AltosState state, AltosListenerState listener_state) { - if (state.gps != null) + if (state.gps != null && state.gps.connected && state.gps.lat != AltosRecord.MISSING) show(pos(state.gps.lat,"N", "S")); else show("???"); @@ -322,7 +322,7 @@ public class AltosAscent extends JComponent implements AltosFlightDisplay { class Lon extends AscentValue { void show (AltosState state, AltosListenerState listener_state) { - if (state.gps != null) + if (state.gps != null && state.gps.connected && state.gps.lon != AltosRecord.MISSING) show(pos(state.gps.lon,"E", "W")); else show("???"); diff --git a/altosui/AltosDescent.java b/altosui/AltosDescent.java index 2b6575cb..35efce16 100644 --- a/altosui/AltosDescent.java +++ b/altosui/AltosDescent.java @@ -278,7 +278,7 @@ public class AltosDescent extends JComponent implements AltosFlightDisplay { class Lat extends DescentValue { void show (AltosState state, AltosListenerState listener_state) { - if (state.gps != null && state.gps.connected) + if (state.gps != null && state.gps.connected && state.gps.lat != AltosRecord.MISSING) show(pos(state.gps.lat,"N", "S")); else show("???"); @@ -292,7 +292,7 @@ public class AltosDescent extends JComponent implements AltosFlightDisplay { class Lon extends DescentValue { void show (AltosState state, AltosListenerState listener_state) { - if (state.gps != null && state.gps.connected) + if (state.gps != null && state.gps.connected && state.gps.lon != AltosRecord.MISSING) show(pos(state.gps.lon,"W", "E")); else show("???"); diff --git a/altosui/AltosDisplayThread.java b/altosui/AltosDisplayThread.java index 70144fb2..7a750c86 100644 --- a/altosui/AltosDisplayThread.java +++ b/altosui/AltosDisplayThread.java @@ -110,7 +110,7 @@ public class AltosDisplayThread extends Thread { */ if (state.state >= Altos.ao_flight_drogue && (last || - System.currentTimeMillis() - state.report_time >= 15000 || + System.currentTimeMillis() - state.received_time >= 15000 || state.state == Altos.ao_flight_landed)) { if (Math.abs(state.speed) < 20 && state.height < 100) diff --git a/altosui/AltosEepromDownload.java b/altosui/AltosEepromDownload.java index 95b17e2a..931b55fd 100644 --- a/altosui/AltosEepromDownload.java +++ b/altosui/AltosEepromDownload.java @@ -33,9 +33,6 @@ public class AltosEepromDownload implements Runnable { Thread eeprom_thread; AltosEepromMonitor monitor; - int flight; - int serial; - int year, month, day; boolean want_file; FileWriter eeprom_file; LinkedList eeprom_pending; @@ -44,7 +41,7 @@ public class AltosEepromDownload implements Runnable { ActionListener listener; boolean success; ParseException parse_exception; - String extension; + AltosState state; private void FlushPending() throws IOException { for (String s : flights.config_data) { @@ -59,15 +56,19 @@ public class AltosEepromDownload implements Runnable { private void CheckFile(boolean force) throws IOException { if (eeprom_file != null) return; - if (force || (flight != 0 && want_file)) { + if (force || (state.flight != 0 && want_file)) { AltosFile eeprom_name; - - if (extension == null) - extension = "data"; - if (year != 0 && month != 0 && day != 0) - eeprom_name = new AltosFile(year, month, day, serial, flight, extension); - else - eeprom_name = new AltosFile(serial, flight, extension); + AltosGPS gps = state.gps; + + if (gps != null && + gps.year != AltosRecord.MISSING && + gps.month != AltosRecord.MISSING && + gps.day != AltosRecord.MISSING) + { + eeprom_name = new AltosFile(gps.year, gps.month, gps.day, + state.serial, state.flight, "eeprom"); + } else + eeprom_name = new AltosFile(state.serial, state.flight, "eeprom"); eeprom_file = new FileWriter(eeprom_name); if (eeprom_file != null) { @@ -78,270 +79,49 @@ public class AltosEepromDownload implements Runnable { } } - void Log(AltosEepromRecord r) throws IOException { + boolean done; + boolean start; + + void LogEeprom(AltosEeprom r) throws IOException { if (r.cmd != Altos.AO_LOG_INVALID) { - String log_line = String.format("%c %4x %4x %4x\n", - r.cmd, r.tick, r.a, r.b); + String line = r.string(); if (eeprom_file != null) - eeprom_file.write(log_line); + eeprom_file.write(line); else - eeprom_pending.add(log_line); + eeprom_pending.add(line); } } - void set_serial(int in_serial) { - serial = in_serial; - monitor.set_serial(serial); - } - - void set_flight(int in_flight) { - flight = in_flight; - monitor.set_flight(flight); - } - - boolean done; - int state; - - void CaptureFull(AltosEepromChunk eechunk) throws IOException { - boolean any_valid = false; - - extension = "eeprom"; - set_serial(flights.config_data.serial); - for (int i = 0; i < AltosEepromChunk.chunk_size && !done; i += AltosEepromRecord.record_length) { - try { - AltosEepromRecord r = new AltosEepromRecord(eechunk, i); - if (r.cmd == Altos.AO_LOG_FLIGHT) - set_flight(r.b); - - /* Monitor state transitions to update display */ - if (r.cmd == Altos.AO_LOG_STATE && r.a <= Altos.ao_flight_landed) { - state = r.a; - if (state > Altos.ao_flight_pad) - want_file = true; - } - - if (r.cmd == Altos.AO_LOG_GPS_DATE) { - year = 2000 + (r.a & 0xff); - month = (r.a >> 8) & 0xff; - day = (r.b & 0xff); - want_file = true; - } - if (r.cmd == Altos.AO_LOG_STATE && r.a == Altos.ao_flight_landed) - done = true; - if (r.cmd != AltosLib.AO_LOG_INVALID) - any_valid = true; - Log(r); - } catch (ParseException pe) { - if (parse_exception == null) - parse_exception = pe; - } - } - - if (!any_valid) - done = true; - - CheckFile(false); - } - - boolean start; - int tiny_tick; - - void CaptureTiny (AltosEepromChunk eechunk) throws IOException { + void CaptureEeprom(AltosEepromChunk eechunk, int log_format) throws IOException { boolean any_valid = false; - extension = "eeprom"; - set_serial(flights.config_data.serial); - for (int i = 0; i < eechunk.data.length && !done; i += 2) { - int v = eechunk.data16(i); - AltosEepromRecord r; - - if (i == 0 && start) { - tiny_tick = 0; - start = false; - set_flight(v); - r = new AltosEepromRecord(Altos.AO_LOG_FLIGHT, tiny_tick, 0, v); - any_valid = true; - } else { - int s = v ^ 0x8000; - - if (Altos.ao_flight_startup <= s && s <= Altos.ao_flight_invalid) { - state = s; - r = new AltosEepromRecord(Altos.AO_LOG_STATE, tiny_tick, state, 0); - if (state == Altos.ao_flight_landed) - done = true; - state = s; - any_valid = true; - } else { - if (v != 0xffff) - any_valid = true; - - r = new AltosEepromRecord(Altos.AO_LOG_PRESSURE, tiny_tick, 0, v); - - /* - * The flight software records ascent data every 100ms, and descent - * data every 1s. - */ - if (state < Altos.ao_flight_drogue) - tiny_tick += 10; - else - tiny_tick += 100; - } - } - Log(r); - } - CheckFile(false); - if (!any_valid) - done = true; - } - - void LogTeleScience(AltosEepromTeleScience r) throws IOException { - if (r.type != Altos.AO_LOG_INVALID) { - String log_line = String.format("%c %4x %4x %d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5d\n", - r.type, r.tick, r.tm_tick, r.tm_state, - r.data[0], r.data[1], r.data[2], r.data[3], - r.data[4], r.data[5], r.data[6], r.data[7], - r.data[8], r.data[9], r.data[10], r.data[11]); - if (eeprom_file != null) - eeprom_file.write(log_line); - else - eeprom_pending.add(log_line); - } - } - - boolean telescience_start; - - void CaptureTeleScience (AltosEepromChunk eechunk) throws IOException { - boolean any_valid = false; - - extension = "science"; - for (int i = 0; i < AltosEepromChunk.chunk_size && !done; i += AltosEepromTeleScience.record_length) { - try { - AltosEepromTeleScience r = new AltosEepromTeleScience(eechunk, i); - if (r.type == AltosEepromTeleScience.AO_LOG_TELESCIENCE_START) { - if (telescience_start) { - done = true; - break; - } - set_serial(r.data[0]); - set_flight(r.data[1]); - telescience_start = true; - } else { - if (!telescience_start) - break; - } - state = r.tm_state; - want_file =true; - any_valid = true; - LogTeleScience(r); - } catch (ParseException pe) { - if (parse_exception == null) - parse_exception = pe; - } - } + int record_length = 8; - CheckFile(false); - if (!any_valid) - done = true; - } + state.set_serial(flights.config_data.serial); - void LogMega(AltosEepromMega r) throws IOException { - if (r.cmd != Altos.AO_LOG_INVALID) { - String log_line = String.format("%c %4x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x\n", - r.cmd, r.tick, - r.data8[0], r.data8[1], r.data8[2], r.data8[3], - r.data8[4], r.data8[5], r.data8[6], r.data8[7], - r.data8[8], r.data8[9], r.data8[10], r.data8[11], - r.data8[12], r.data8[13], r.data8[14], r.data8[15], - r.data8[16], r.data8[17], r.data8[18], r.data8[19], - r.data8[20], r.data8[21], r.data8[22], r.data8[23], - r.data8[24], r.data8[25], r.data8[26], r.data8[27]); - if (eeprom_file != null) - eeprom_file.write(log_line); - else - eeprom_pending.add(log_line); - } - } + for (int i = 0; i < AltosEepromChunk.chunk_size && !done; i += record_length) { + AltosEeprom r = eechunk.eeprom(i, log_format); - void CaptureMega(AltosEepromChunk eechunk) throws IOException { - boolean any_valid = false; + record_length = r.record_length(); - extension = "mega"; - set_serial(flights.config_data.serial); - for (int i = 0; i < AltosEepromChunk.chunk_size && !done; i += AltosEepromMega.record_length) { - try { - AltosEepromMega r = new AltosEepromMega(eechunk, i); - if (r.cmd == Altos.AO_LOG_FLIGHT) - set_flight(r.data16(0)); - - /* Monitor state transitions to update display */ - if (r.cmd == Altos.AO_LOG_STATE && r.data16(0) <= Altos.ao_flight_landed) { - state = r.data16(0); - if (state > Altos.ao_flight_pad) - want_file = true; - } + r.update_state(state); - if (r.cmd == Altos.AO_LOG_GPS_TIME) { - year = 2000 + r.data8(14); - month = r.data8(15); - day = r.data8(16); + /* Monitor state transitions to update display */ + if (state.state != AltosLib.ao_flight_invalid && + state.state <= AltosLib.ao_flight_landed) + { + if (state.state > Altos.ao_flight_pad) want_file = true; - } - - if (r.cmd == Altos.AO_LOG_STATE && r.data16(0) == Altos.ao_flight_landed) + if (state.state == AltosLib.ao_flight_landed) done = true; - if (r.cmd != AltosLib.AO_LOG_INVALID) - any_valid = true; - LogMega(r); - } catch (ParseException pe) { - if (parse_exception == null) - parse_exception = pe; } - } - if (!any_valid) - done = true; - CheckFile(false); - } - - void LogMini(AltosEepromMini r) throws IOException { - if (r.cmd != Altos.AO_LOG_INVALID) { - String log_line = String.format("%c %4x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x %2x\n", - r.cmd, r.tick, - r.data8[0], r.data8[1], r.data8[2], r.data8[3], - r.data8[4], r.data8[5], r.data8[6], r.data8[7], - r.data8[8], r.data8[9], r.data8[10], r.data8[11]); - if (eeprom_file != null) - eeprom_file.write(log_line); - else - eeprom_pending.add(log_line); - } - } + if (state.gps != null) + want_file = true; - void CaptureMini(AltosEepromChunk eechunk) throws IOException { - boolean any_valid = false; - - extension = "mini"; - set_serial(flights.config_data.serial); - for (int i = 0; i < AltosEepromChunk.chunk_size && !done; i += AltosEepromMini.record_length) { - try { - AltosEepromMini r = new AltosEepromMini(eechunk, i); - if (r.cmd == Altos.AO_LOG_FLIGHT) - set_flight(r.data16(0)); - - /* Monitor state transitions to update display */ - if (r.cmd == Altos.AO_LOG_STATE && r.data16(0) <= Altos.ao_flight_landed) { - state = r.data16(0); - if (state > Altos.ao_flight_pad) - want_file = true; - } - - if (r.cmd == Altos.AO_LOG_STATE && r.data16(0) == Altos.ao_flight_landed) - done = true; + if (r.valid) { any_valid = true; - LogMini(r); - } catch (ParseException pe) { - if (parse_exception == null) - parse_exception = pe; + LogEeprom(r); } } if (!any_valid) @@ -350,15 +130,12 @@ public class AltosEepromDownload implements Runnable { CheckFile(false); } - void CaptureTelemetry(AltosEepromChunk eechunk) throws IOException { - - } - void CaptureLog(AltosEepromLog log) throws IOException, InterruptedException, TimeoutException { int block, state_block = 0; int log_format = flights.config_data.log_format; - state = 0; + state = new AltosState(); + done = false; start = true; @@ -366,10 +143,6 @@ public class AltosEepromDownload implements Runnable { throw new IOException("no serial number found"); /* Reset per-capture variables */ - flight = 0; - year = 0; - month = 0; - day = 0; want_file = false; eeprom_file = null; eeprom_pending = new LinkedList(); @@ -377,9 +150,12 @@ public class AltosEepromDownload implements Runnable { /* Set serial number in the monitor dialog window */ /* Now scan the eeprom, reading blocks of data and converting to .eeprom file form */ - state = 0; state_block = log.start_block; + state_block = log.start_block; for (block = log.start_block; !done && block < log.end_block; block++) { - monitor.set_value(AltosLib.state_name(state), state, block - state_block, block - log.start_block); + monitor.set_value(state.state_name(), + state.state, + block - state_block, + block - log.start_block); AltosEepromChunk eechunk = new AltosEepromChunk(serial_line, block, block == log.start_block); @@ -397,33 +173,7 @@ public class AltosEepromDownload implements Runnable { } } - switch (log_format) { - case AltosLib.AO_LOG_FORMAT_FULL: - extension = "eeprom"; - CaptureFull(eechunk); - break; - case AltosLib.AO_LOG_FORMAT_TINY: - extension = "eeprom"; - CaptureTiny(eechunk); - break; - case AltosLib.AO_LOG_FORMAT_TELEMETRY: - extension = "telem"; - CaptureTelemetry(eechunk); - break; - case AltosLib.AO_LOG_FORMAT_TELESCIENCE: - extension = "science"; - CaptureTeleScience(eechunk); - break; - case AltosLib.AO_LOG_FORMAT_TELEMEGA: - extension = "mega"; - CaptureMega(eechunk); - break; - case AltosLib.AO_LOG_FORMAT_EASYMINI: - case AltosLib.AO_LOG_FORMAT_TELEMINI: - extension = "eeprom"; - CaptureMini(eechunk); - break; - } + CaptureEeprom (eechunk, log_format); } CheckFile(true); if (eeprom_file != null) { diff --git a/altosui/AltosFlightStatus.java b/altosui/AltosFlightStatus.java index 0be7bb51..6383e5b9 100644 --- a/altosui/AltosFlightStatus.java +++ b/altosui/AltosFlightStatus.java @@ -126,7 +126,7 @@ public class AltosFlightStatus extends JComponent implements AltosFlightDisplay class LastPacket extends FlightValue { void show(AltosState state, AltosListenerState listener_state) { - long secs = (System.currentTimeMillis() - state.report_time + 500) / 1000; + long secs = (System.currentTimeMillis() - state.received_time + 500) / 1000; value.setText(String.format("%d", secs)); } public LastPacket(GridBagLayout layout, int x) { diff --git a/altosui/AltosFlightUI.java b/altosui/AltosFlightUI.java index 423cf10c..1c450ce3 100644 --- a/altosui/AltosFlightUI.java +++ b/altosui/AltosFlightUI.java @@ -102,7 +102,7 @@ public class AltosFlightUI extends AltosUIFrame implements AltosFlightDisplay, A status_update.saved_state = state; if (state == null) - state = new AltosState(new AltosRecord()); + state = new AltosState(); pad.show(state, listener_state); diff --git a/altosui/AltosIdleMonitorUI.java b/altosui/AltosIdleMonitorUI.java index bbab017f..f6a91de8 100644 --- a/altosui/AltosIdleMonitorUI.java +++ b/altosui/AltosIdleMonitorUI.java @@ -65,13 +65,13 @@ public class AltosIdleMonitorUI extends AltosUIFrame implements AltosFlightDispl public void show(AltosState state, AltosListenerState listener_state) { status_update.saved_state = state; - try { +// try { pad.show(state, listener_state); flightStatus.show(state, listener_state); flightInfo.show(state, listener_state); - } catch (Exception e) { - System.out.print("Show exception" + e); - } +// } catch (Exception e) { +// System.out.print("Show exception " + e); +// } } public void update(final AltosState state, final AltosListenerState listener_state) { diff --git a/altosui/AltosInfoTable.java b/altosui/AltosInfoTable.java index 8601d76f..8906920b 100644 --- a/altosui/AltosInfoTable.java +++ b/altosui/AltosInfoTable.java @@ -155,10 +155,14 @@ public class AltosInfoTable extends JTable { else info_add_row(1, "GPS", " missing"); info_add_row(1, "Satellites", "%6d", state.gps.nsat); - info_add_deg(1, "Latitude", state.gps.lat, 'N', 'S'); - info_add_deg(1, "Longitude", state.gps.lon, 'E', 'W'); - info_add_row(1, "GPS altitude", "%6d", state.gps.alt); - info_add_row(1, "GPS height", "%6.0f", state.gps_height); + if (state.gps.lat != AltosRecord.MISSING) + info_add_deg(1, "Latitude", state.gps.lat, 'N', 'S'); + if (state.gps.lon != AltosRecord.MISSING) + info_add_deg(1, "Longitude", state.gps.lon, 'E', 'W'); + if (state.gps.alt != AltosRecord.MISSING) + info_add_row(1, "GPS altitude", "%8.1f", state.gps.alt); + if (state.gps_height != AltosRecord.MISSING) + info_add_row(1, "GPS height", "%8.1f", state.gps_height); /* The SkyTraq GPS doesn't report these values */ /* @@ -195,14 +199,16 @@ public class AltosInfoTable extends JTable { info_add_deg(1, "Pad longitude", state.pad_lon, 'E', 'W'); info_add_row(1, "Pad GPS alt", "%6.0f m", state.pad_alt); } - info_add_row(1, "GPS date", "%04d-%02d-%02d", - state.gps.year, - state.gps.month, - state.gps.day); - info_add_row(1, "GPS time", " %02d:%02d:%02d", - state.gps.hour, - state.gps.minute, - state.gps.second); + if (state.gps.year != AltosRecord.MISSING) + info_add_row(1, "GPS date", "%04d-%02d-%02d", + state.gps.year, + state.gps.month, + state.gps.day); + if (state.gps.hour != AltosRecord.MISSING) + info_add_row(1, "GPS time", " %02d:%02d:%02d", + state.gps.hour, + state.gps.minute, + state.gps.second); //int nsat_vis = 0; int c; diff --git a/altosui/AltosLanded.java b/altosui/AltosLanded.java index 38f273cf..4cdaa3df 100644 --- a/altosui/AltosLanded.java +++ b/altosui/AltosLanded.java @@ -103,7 +103,8 @@ public class AltosLanded extends JComponent implements AltosFlightDisplay, Actio class Lat extends LandedValue { void show (AltosState state, AltosListenerState listener_state) { - if (state.gps != null && state.gps.connected) + show(); + if (state.gps != null && state.gps.connected && state.gps.lat != AltosRecord.MISSING) show(pos(state.gps.lat,"N", "S")); else show("???"); @@ -118,7 +119,7 @@ public class AltosLanded extends JComponent implements AltosFlightDisplay, Actio class Lon extends LandedValue { void show (AltosState state, AltosListenerState listener_state) { show(); - if (state.gps != null && state.gps.connected) + if (state.gps != null && state.gps.connected && state.gps.lon != AltosRecord.MISSING) show(pos(state.gps.lon,"E", "W")); else show("???"); diff --git a/altosui/AltosPad.java b/altosui/AltosPad.java index fed009cc..e9c973de 100644 --- a/altosui/AltosPad.java +++ b/altosui/AltosPad.java @@ -310,17 +310,23 @@ public class AltosPad extends JComponent implements AltosFlightDisplay { class PadLat extends LaunchValue { void show (AltosState state, AltosListenerState listener_state) { - if (state == null || state.gps == null) { - hide(); - } else { - if (state.state < AltosLib.ao_flight_pad) { - show(pos(state.gps.lat,"N", "S")); - set_label("Latitude"); - } else { - show(pos(state.pad_lat,"N", "S")); - set_label("Pad Latitude"); + double lat = AltosRecord.MISSING; + String label = null; + + if (state != null) { + if (state.state < AltosLib.ao_flight_pad && state.gps != null && state.gps.lat != AltosRecord.MISSING) { + lat = state.gps.lat; + label = "Latitude"; + } else { + lat = state.pad_lat; + label = "Pad Latitude"; } } + if (lat != AltosRecord.MISSING) { + show(pos(lat,"E", "W")); + set_label(label); + } else + hide(); } public PadLat (GridBagLayout layout, int y) { super (layout, y, "Pad Latitude"); @@ -331,17 +337,23 @@ public class AltosPad extends JComponent implements AltosFlightDisplay { class PadLon extends LaunchValue { void show (AltosState state, AltosListenerState listener_state) { - if (state == null || state.gps == null) { - hide(); - } else { - if (state.state < AltosLib.ao_flight_pad) { - show(pos(state.gps.lon,"E", "W")); - set_label("Longitude"); - } else { - show(pos(state.pad_lon,"E", "W")); - set_label("Pad Longitude"); + double lon = AltosRecord.MISSING; + String label = null; + + if (state != null) { + if (state.state < AltosLib.ao_flight_pad && state.gps != null && state.gps.lon != AltosRecord.MISSING) { + lon = state.gps.lon; + label = "Longitude"; + } else { + lon = state.pad_lon; + label = "Pad Longitude"; } } + if (lon != AltosRecord.MISSING) { + show(pos(lon,"E", "W")); + set_label(label); + } else + hide(); } public PadLon (GridBagLayout layout, int y) { super (layout, y, "Pad Longitude"); @@ -352,21 +364,23 @@ public class AltosPad extends JComponent implements AltosFlightDisplay { class PadAlt extends LaunchValue { void show (AltosState state, AltosListenerState listener_state) { - if (state == null) - hide(); - else { - if (state.state < AltosLib.ao_flight_pad && state.gps != null) { - show("%4.0f m", state.gps.alt); - set_label("Altitude"); + double alt = AltosRecord.MISSING; + String label = null; + + if (state != null) { + if (state.state < AltosLib.ao_flight_pad && state.gps != null && state.gps.alt != AltosRecord.MISSING) { + alt = state.gps.alt; + label = "Altitude"; } else { - if (state.pad_alt == AltosRecord.MISSING) - hide(); - else { - show("%4.0f m", state.pad_alt); - set_label("Pad Altitude"); - } + alt = state.pad_alt; + label = "Pad Altitude"; } } + if (alt != AltosRecord.MISSING) { + show("%4.0f m", state.gps.alt); + set_label(label); + } else + hide(); } public PadAlt (GridBagLayout layout, int y) { super (layout, y, "Pad Altitude"); diff --git a/altosui/AltosUI.java b/altosui/AltosUI.java index b47df0d9..151f68fd 100644 --- a/altosui/AltosUI.java +++ b/altosui/AltosUI.java @@ -350,10 +350,10 @@ public class AltosUI extends AltosUIFrame { FileInputStream in; in = new FileInputStream(file); - if (file.getName().endsWith("eeprom")) - return new AltosEepromFile(in); - else + if (file.getName().endsWith("telem")) return new AltosTelemetryFile(in); + else + return new AltosEepromFile(in); } catch (FileNotFoundException fe) { System.out.printf("%s\n", fe.getMessage()); return null; @@ -434,11 +434,10 @@ public class AltosUI extends AltosUIFrame { System.out.printf("Failed to open file '%s'\n", file); return null; } - if (file.getName().endsWith("eeprom")) { - return new AltosEepromFile(in); - } else { + if (file.getName().endsWith("telem")) return new AltosTelemetryFile(in); - } + else + return new AltosEepromFile(in); } static AltosReplayReader replay_file(File file) { @@ -521,6 +520,8 @@ public class AltosUI extends AltosUIFrame { System.out.printf ("process cat\n"); for (AltosState state : eef) { + System.out.printf ("tick %d state %d height %g\n", + state.tick, state.state, state.height); if ((state.set & AltosState.set_gps) != 0) System.out.printf ("time %g lat %g lon %g alt %g\n", state.time_since_boost(), diff --git a/src/core/ao_log.h b/src/core/ao_log.h index a2f342d7..4b09faeb 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -276,7 +276,8 @@ struct ao_log_metrum { uint16_t flight; /* 4 */ int16_t ground_accel; /* 6 */ uint32_t ground_pres; /* 8 */ - } flight; /* 12 */ + uint32_t ground_temp; /* 12 */ + } flight; /* 16 */ /* AO_LOG_STATE */ struct { uint16_t state; /* 4 */ -- cgit v1.2.3 From 7f4650990e8a7cfcf8461e8928dfc426c9a563cc Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 5 Sep 2013 22:57:19 -0700 Subject: altos: Set tick value in new TeleMetrum v2 sensor packets Was getting left with the old value, which wasn't very useful Signed-off-by: Keith Packard --- src/core/ao_telemetry.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src/core') diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index cd95aa6b..6b47a06a 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -182,6 +182,7 @@ 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; -- cgit v1.2.3 From 690094e2d7d9cfe5eb4edb478fd79e5d133c6b4b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 15 Sep 2013 14:11:50 -0700 Subject: altos: Move micropeak sources around This sticks the micropeak sources in appropriate directories, rather than in the micropeak product directory so that they can be shared with future micropeak-style products. Signed-off-by: Keith Packard --- src/attiny/ao_async.c | 71 ++++++++++++++++++++ src/attiny/ao_async.h | 30 +++++++++ src/core/ao_log_micro.c | 121 ++++++++++++++++++++++++++++++++++ src/core/ao_log_micro.h | 39 +++++++++++ src/core/ao_microflight.c | 143 +++++++++++++++++++++++++++++++++++++++++ src/core/ao_microkalman.c | 74 +++++++++++++++++++++ src/core/ao_report_micro.c | 57 ++++++++++++++++ src/micropeak/Makefile | 6 +- src/micropeak/ao_async.c | 71 -------------------- src/micropeak/ao_async.h | 30 --------- src/micropeak/ao_log_micro.c | 121 ---------------------------------- src/micropeak/ao_log_micro.h | 39 ----------- src/micropeak/ao_microflight.c | 143 ----------------------------------------- src/micropeak/ao_microkalman.c | 74 --------------------- src/micropeak/ao_micropeak.c | 89 ------------------------- src/micropeak/ao_micropeak.h | 78 ---------------------- src/micropeak/ao_report_tiny.c | 57 ---------------- src/product/ao_micropeak.c | 89 +++++++++++++++++++++++++ src/product/ao_micropeak.h | 78 ++++++++++++++++++++++ 19 files changed, 705 insertions(+), 705 deletions(-) create mode 100644 src/attiny/ao_async.c create mode 100644 src/attiny/ao_async.h create mode 100644 src/core/ao_log_micro.c create mode 100644 src/core/ao_log_micro.h create mode 100644 src/core/ao_microflight.c create mode 100644 src/core/ao_microkalman.c create mode 100644 src/core/ao_report_micro.c delete mode 100644 src/micropeak/ao_async.c delete mode 100644 src/micropeak/ao_async.h delete mode 100644 src/micropeak/ao_log_micro.c delete mode 100644 src/micropeak/ao_log_micro.h delete mode 100644 src/micropeak/ao_microflight.c delete mode 100644 src/micropeak/ao_microkalman.c delete mode 100644 src/micropeak/ao_micropeak.c delete mode 100644 src/micropeak/ao_micropeak.h delete mode 100644 src/micropeak/ao_report_tiny.c create mode 100644 src/product/ao_micropeak.c create mode 100644 src/product/ao_micropeak.h (limited to 'src/core') diff --git a/src/attiny/ao_async.c b/src/attiny/ao_async.c new file mode 100644 index 00000000..3556f54c --- /dev/null +++ b/src/attiny/ao_async.c @@ -0,0 +1,71 @@ +/* + * 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 + +#define AO_ASYNC_BAUD 38400l +#define AO_ASYNC_DELAY (uint8_t) (1000000l / AO_ASYNC_BAUD) + +#define LED_PORT PORTB + +void +ao_async_start(void) +{ + LED_PORT |= (1 << AO_LED_SERIAL); +} + +void +ao_async_stop(void) +{ + LED_PORT &= ~(1 << AO_LED_SERIAL); +} + +void +ao_async_byte(uint8_t byte) +{ + uint8_t b; + uint16_t w; + + /* start data stop */ + w = (0x000 << 0) | (byte << 1) | (0x001 << 9); + + ao_arch_block_interrupts(); + for (b = 0; b < 10; b++) { + uint8_t v = LED_PORT & ~(1 << AO_LED_SERIAL); + v |= (w & 1) << AO_LED_SERIAL; + LED_PORT = v; + w >>= 1; + + /* Carefully timed to hit around 9600 baud */ + asm volatile ("nop"); + asm volatile ("nop"); + + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + } + ao_arch_release_interrupts(); +} diff --git a/src/attiny/ao_async.h b/src/attiny/ao_async.h new file mode 100644 index 00000000..1b239712 --- /dev/null +++ b/src/attiny/ao_async.h @@ -0,0 +1,30 @@ +/* + * 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_ASYNC_H_ +#define _AO_ASYNC_H_ + +void +ao_async_start(void); + +void +ao_async_stop(void); + +void +ao_async_byte(uint8_t byte); + +#endif /* _AO_ASYNC_H_ */ diff --git a/src/core/ao_log_micro.c b/src/core/ao_log_micro.c new file mode 100644 index 00000000..d665efb5 --- /dev/null +++ b/src/core/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/core/ao_log_micro.h b/src/core/ao_log_micro.h new file mode 100644 index 00000000..976852ee --- /dev/null +++ b/src/core/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/core/ao_microflight.c b/src/core/ao_microflight.c new file mode 100644 index 00000000..714bb90a --- /dev/null +++ b/src/core/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 16 + +#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 == 0) + ao_led_on(AO_LED_REPORT); + ao_delay_until(time); + ao_microsample(); + if (sample_count == 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 first sample a decent interval above the ground */ + pa_min = pa_ground - LAND_DETECT; + for (i = SKIP_PA_HIST(h,2); i != h; 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 new file mode 100644 index 00000000..0684ea2b --- /dev/null +++ b/src/core/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/core/ao_report_micro.c b/src/core/ao_report_micro.c new file mode 100644 index 00000000..0e8e287f --- /dev/null +++ b/src/core/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/micropeak/Makefile b/src/micropeak/Makefile index 44e0b873..e51b2847 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -2,7 +2,7 @@ # Tiny AltOS build # # -vpath % ../attiny:../drivers:../core:.. +vpath % ../attiny:../drivers:../core:../product:.. vpath ao-make-product.5c ../util vpath make-altitude-pa ../util @@ -30,7 +30,7 @@ ALTOS_SRC = \ ao_ms5607.c \ ao_exti.c \ ao_convert_pa.c \ - ao_report_tiny.c \ + ao_report_micro.c \ ao_notask.c \ ao_eeprom_tiny.c \ ao_panic.c \ @@ -53,7 +53,7 @@ INC=\ IDPRODUCT=0 PRODUCT=MicroPeak-v0.1 PRODUCT_DEF=-DMICROPEAK -CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers +CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -I../product CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O2 -mcall-prologues -DATTINY NICKLE=nickle diff --git a/src/micropeak/ao_async.c b/src/micropeak/ao_async.c deleted file mode 100644 index 3556f54c..00000000 --- a/src/micropeak/ao_async.c +++ /dev/null @@ -1,71 +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 - -#define AO_ASYNC_BAUD 38400l -#define AO_ASYNC_DELAY (uint8_t) (1000000l / AO_ASYNC_BAUD) - -#define LED_PORT PORTB - -void -ao_async_start(void) -{ - LED_PORT |= (1 << AO_LED_SERIAL); -} - -void -ao_async_stop(void) -{ - LED_PORT &= ~(1 << AO_LED_SERIAL); -} - -void -ao_async_byte(uint8_t byte) -{ - uint8_t b; - uint16_t w; - - /* start data stop */ - w = (0x000 << 0) | (byte << 1) | (0x001 << 9); - - ao_arch_block_interrupts(); - for (b = 0; b < 10; b++) { - uint8_t v = LED_PORT & ~(1 << AO_LED_SERIAL); - v |= (w & 1) << AO_LED_SERIAL; - LED_PORT = v; - w >>= 1; - - /* Carefully timed to hit around 9600 baud */ - asm volatile ("nop"); - asm volatile ("nop"); - - asm volatile ("nop"); - asm volatile ("nop"); - asm volatile ("nop"); - asm volatile ("nop"); - asm volatile ("nop"); - - asm volatile ("nop"); - asm volatile ("nop"); - asm volatile ("nop"); - asm volatile ("nop"); - asm volatile ("nop"); - } - ao_arch_release_interrupts(); -} diff --git a/src/micropeak/ao_async.h b/src/micropeak/ao_async.h deleted file mode 100644 index 1b239712..00000000 --- a/src/micropeak/ao_async.h +++ /dev/null @@ -1,30 +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_ASYNC_H_ -#define _AO_ASYNC_H_ - -void -ao_async_start(void); - -void -ao_async_stop(void); - -void -ao_async_byte(uint8_t byte); - -#endif /* _AO_ASYNC_H_ */ diff --git a/src/micropeak/ao_log_micro.c b/src/micropeak/ao_log_micro.c deleted file mode 100644 index d665efb5..00000000 --- a/src/micropeak/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/micropeak/ao_log_micro.h b/src/micropeak/ao_log_micro.h deleted file mode 100644 index 976852ee..00000000 --- a/src/micropeak/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/micropeak/ao_microflight.c b/src/micropeak/ao_microflight.c deleted file mode 100644 index 714bb90a..00000000 --- a/src/micropeak/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 16 - -#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 == 0) - ao_led_on(AO_LED_REPORT); - ao_delay_until(time); - ao_microsample(); - if (sample_count == 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 first sample a decent interval above the ground */ - pa_min = pa_ground - LAND_DETECT; - for (i = SKIP_PA_HIST(h,2); i != h; 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/micropeak/ao_microkalman.c b/src/micropeak/ao_microkalman.c deleted file mode 100644 index 0684ea2b..00000000 --- a/src/micropeak/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/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c deleted file mode 100644 index 10f0d192..00000000 --- a/src/micropeak/ao_micropeak.c +++ /dev/null @@ -1,89 +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 -#include - -static struct ao_ms5607_sample sample; -static struct ao_ms5607_value value; - -alt_t ground_alt, max_alt; -alt_t ao_max_height; - -void -ao_pa_get(void) -{ - ao_ms5607_sample(&sample); - ao_ms5607_convert(&sample, &value); - pa = value.pres; -} - -static void -ao_compute_height(void) -{ - ground_alt = ao_pa_to_altitude(pa_ground); - max_alt = ao_pa_to_altitude(pa_min); - ao_max_height = max_alt - ground_alt; -} - -static void -ao_pips(void) -{ - uint8_t i; - for (i = 0; i < 10; i++) { - ao_led_toggle(AO_LED_REPORT); - ao_delay(AO_MS_TO_TICKS(80)); - } - ao_delay(AO_MS_TO_TICKS(200)); -} - -int -main(void) -{ - ao_led_init(LEDS_AVAILABLE); - ao_timer_init(); - - /* Init external hardware */ - ao_spi_init(); - ao_ms5607_init(); - ao_ms5607_setup(); - - /* Give the person a second to get their finger out of the way */ - ao_delay(AO_MS_TO_TICKS(1000)); - - ao_log_micro_restore(); - ao_compute_height(); - ao_report_altitude(); - ao_pips(); - ao_log_micro_dump(); - - ao_delay(BOOST_DELAY); - - ao_microflight(); - - ao_log_micro_save(); - ao_compute_height(); - ao_report_altitude(); - for (;;) { - cli(); - set_sleep_mode(SLEEP_MODE_PWR_DOWN); - sleep_mode(); - } -} diff --git a/src/micropeak/ao_micropeak.h b/src/micropeak/ao_micropeak.h deleted file mode 100644 index 382b98d9..00000000 --- a/src/micropeak/ao_micropeak.h +++ /dev/null @@ -1,78 +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_MICROPEAK_H_ -#define _AO_MICROPEAK_H_ - -#define SAMPLE_SLEEP AO_MS_TO_TICKS(96) - -/* 16 sample, or about two seconds worth */ -#define GROUND_AVG_SHIFT 4 -#define GROUND_AVG (1 << GROUND_AVG_SHIFT) - -/* Pressure change (in Pa) to detect boost */ -#define BOOST_DETECT 120 /* 10m at sea level, 12m at 2000m */ - -/* Wait after power on before doing anything to give the user time to assemble the rocket */ -#define BOOST_DELAY AO_SEC_TO_TICKS(30) - -/* Pressure change (in Pa) to detect landing */ -#define LAND_DETECT 24 /* 2m at sea level, 2.4m at 2000m */ - -/* Current sensor pressure value */ -extern uint32_t pa; - -/* Average pressure value on ground */ -extern uint32_t pa_ground; - -/* Minimum recorded filtered pressure value */ -extern uint32_t pa_min; - -/* Pressure values converted to altitudes */ -extern alt_t ground_alt, max_alt; - -/* max_alt - ground_alt */ -extern alt_t ao_max_height; - -void -ao_pa_get(void); - -void -ao_microflight(void); - -#define ACCEL_LOCK_PA -20 -#define ACCEL_LOCK_TIME 10 - -extern uint32_t ao_k_pa; /* 24.8 fixed point */ -extern int32_t ao_k_pa_speed; /* 16.16 fixed point */ -extern int32_t ao_k_pa_accel; /* 16.16 fixed point */ - -extern uint32_t ao_pa; /* integer portion */ -extern int16_t ao_pa_speed; /* integer portion */ -extern int16_t ao_pa_accel; /* integer portion */ - -void -ao_microkalman_init(void); - -void -ao_microkalman_predict(void); - -void -ao_microkalman_correct(void); - -#endif - diff --git a/src/micropeak/ao_report_tiny.c b/src/micropeak/ao_report_tiny.c deleted file mode 100644 index 0e8e287f..00000000 --- a/src/micropeak/ao_report_tiny.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/product/ao_micropeak.c b/src/product/ao_micropeak.c new file mode 100644 index 00000000..10f0d192 --- /dev/null +++ b/src/product/ao_micropeak.c @@ -0,0 +1,89 @@ +/* + * 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 +#include + +static struct ao_ms5607_sample sample; +static struct ao_ms5607_value value; + +alt_t ground_alt, max_alt; +alt_t ao_max_height; + +void +ao_pa_get(void) +{ + ao_ms5607_sample(&sample); + ao_ms5607_convert(&sample, &value); + pa = value.pres; +} + +static void +ao_compute_height(void) +{ + ground_alt = ao_pa_to_altitude(pa_ground); + max_alt = ao_pa_to_altitude(pa_min); + ao_max_height = max_alt - ground_alt; +} + +static void +ao_pips(void) +{ + uint8_t i; + for (i = 0; i < 10; i++) { + ao_led_toggle(AO_LED_REPORT); + ao_delay(AO_MS_TO_TICKS(80)); + } + ao_delay(AO_MS_TO_TICKS(200)); +} + +int +main(void) +{ + ao_led_init(LEDS_AVAILABLE); + ao_timer_init(); + + /* Init external hardware */ + ao_spi_init(); + ao_ms5607_init(); + ao_ms5607_setup(); + + /* Give the person a second to get their finger out of the way */ + ao_delay(AO_MS_TO_TICKS(1000)); + + ao_log_micro_restore(); + ao_compute_height(); + ao_report_altitude(); + ao_pips(); + ao_log_micro_dump(); + + ao_delay(BOOST_DELAY); + + ao_microflight(); + + ao_log_micro_save(); + ao_compute_height(); + ao_report_altitude(); + for (;;) { + cli(); + set_sleep_mode(SLEEP_MODE_PWR_DOWN); + sleep_mode(); + } +} diff --git a/src/product/ao_micropeak.h b/src/product/ao_micropeak.h new file mode 100644 index 00000000..3e3dec15 --- /dev/null +++ b/src/product/ao_micropeak.h @@ -0,0 +1,78 @@ +/* + * 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_MICROPEAK_H_ +#define _AO_MICROPEAK_H_ + +#define SAMPLE_SLEEP AO_MS_TO_TICKS(96) + +/* 16 sample, or about two seconds worth */ +#define GROUND_AVG_SHIFT 4 +#define GROUND_AVG (1 << GROUND_AVG_SHIFT) + +/* Pressure change (in Pa) to detect boost */ +#define BOOST_DETECT 120 /* 10m at sea level, 12m at 2000m */ + +/* Wait after power on before doing anything to give the user time to assemble the rocket */ +#define BOOST_DELAY AO_SEC_TO_TICKS(60) + +/* Pressure change (in Pa) to detect landing */ +#define LAND_DETECT 24 /* 2m at sea level, 2.4m at 2000m */ + +/* Current sensor pressure value */ +extern uint32_t pa; + +/* Average pressure value on ground */ +extern uint32_t pa_ground; + +/* Minimum recorded filtered pressure value */ +extern uint32_t pa_min; + +/* Pressure values converted to altitudes */ +extern alt_t ground_alt, max_alt; + +/* max_alt - ground_alt */ +extern alt_t ao_max_height; + +void +ao_pa_get(void); + +void +ao_microflight(void); + +#define ACCEL_LOCK_PA -20 +#define ACCEL_LOCK_TIME 10 + +extern uint32_t ao_k_pa; /* 24.8 fixed point */ +extern int32_t ao_k_pa_speed; /* 16.16 fixed point */ +extern int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +extern uint32_t ao_pa; /* integer portion */ +extern int16_t ao_pa_speed; /* integer portion */ +extern int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void); + +void +ao_microkalman_predict(void); + +void +ao_microkalman_correct(void); + +#endif + -- cgit v1.2.3 From 4254de22864de2ed7ae5928c6b8bfd9df1c8a3fb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 7 Oct 2013 21:51:30 -0700 Subject: altos: Don't require an LED for ao_flight EasyMini has no LEDs. Deal with it. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/core') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 1322195b..88dc816d 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -134,8 +134,10 @@ ao_flight(void) ao_rdf_set(1); ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); #endif +#if HAS_LED /* 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; @@ -145,8 +147,10 @@ ao_flight(void) ao_packet_slave_start(); #endif +#if HAS_LED /* 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)); -- cgit v1.2.3 From 1bd9786802751391cca3b83ac3045029e00e39ee Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 13 Oct 2013 22:05:20 -0700 Subject: altos/micropeak: Increase boost detect to 30m This meant increasing the data buffering as well so that we could reliably capture the flight data back to the ground, even for slow flights. And, with the buffer extra large, we work backwards from the current buffer location to find the last ground location rather than working forwards from the first buffered location. This ensures that we don't capture noise before boost and instead capture a nice flight curve instead. Signed-off-by: Keith Packard --- src/core/ao_microflight.c | 12 ++++++------ src/product/ao_micropeak.h | 6 +++--- 2 files changed, 9 insertions(+), 9 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_microflight.c b/src/core/ao_microflight.c index 714bb90a..f680e400 100644 --- a/src/core/ao_microflight.c +++ b/src/core/ao_microflight.c @@ -33,7 +33,7 @@ ao_microsample(void) ao_microkalman_correct(); } -#define NUM_PA_HIST 16 +#define NUM_PA_HIST (GROUND_AVG) #define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) @@ -60,11 +60,11 @@ ao_microflight(void) h = 0; for (;;) { time += SAMPLE_SLEEP; - if (sample_count == 0) + if ((sample_count & 0x1f) == 0) ao_led_on(AO_LED_REPORT); ao_delay_until(time); ao_microsample(); - if (sample_count == 0) + if ((sample_count & 0x1f) == 0) ao_led_off(AO_LED_REPORT); pa_hist[h] = pa; h = SKIP_PA_HIST(h,1); @@ -85,10 +85,10 @@ ao_microflight(void) } } - /* Go back and find the first sample a decent interval above the ground */ + /* 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 != h; i = SKIP_PA_HIST(i,2)) { - if (pa_hist[i] < pa_min) + 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; } diff --git a/src/product/ao_micropeak.h b/src/product/ao_micropeak.h index 3e3dec15..0cefca6f 100644 --- a/src/product/ao_micropeak.h +++ b/src/product/ao_micropeak.h @@ -20,12 +20,12 @@ #define SAMPLE_SLEEP AO_MS_TO_TICKS(96) -/* 16 sample, or about two seconds worth */ -#define GROUND_AVG_SHIFT 4 +/* 64 sample, or about six seconds worth */ +#define GROUND_AVG_SHIFT 6 #define GROUND_AVG (1 << GROUND_AVG_SHIFT) /* Pressure change (in Pa) to detect boost */ -#define BOOST_DETECT 120 /* 10m at sea level, 12m at 2000m */ +#define BOOST_DETECT 360 /* 30m at sea level, 36m at 2000m */ /* Wait after power on before doing anything to give the user time to assemble the rocket */ #define BOOST_DELAY AO_SEC_TO_TICKS(60) -- cgit v1.2.3 From 039446f54ef6968a3f0b37ce32ca6bdcdbe62546 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 14 Oct 2013 22:41:43 -0700 Subject: altos: Merge GPS logging into a single function Create a new global, ao_gps_new, which indicates new GPS position and satellite data. Use ao_gps_new as the new sleep/wakeup address. Merge the separate gps position/satellite logging tasks into a single function which waits for new data and writes out the changed values. Signed-off-by: Keith Packard --- src/core/ao.h | 4 ++ src/core/ao_flight.c | 4 +- src/core/ao_gps_report.c | 102 ++++++++++++++++------------------- src/core/ao_gps_report_mega.c | 88 +++++++++++++------------------ src/core/ao_gps_report_metrum.c | 108 +++++++++++++++++--------------------- src/drivers/ao_gps_sirf.c | 7 ++- src/drivers/ao_gps_skytraq.c | 7 ++- src/drivers/ao_gps_ublox.c | 5 +- src/product/ao_terraui.c | 2 +- src/teleballoon-v1.1/ao_balloon.c | 4 +- src/test/ao_gps_test.c | 13 +++-- src/test/ao_gps_test_skytraq.c | 2 +- src/test/ao_gps_test_ublox.c | 2 +- 13 files changed, 164 insertions(+), 184 deletions(-) (limited to 'src/core') diff --git a/src/core/ao.h b/src/core/ao.h index e7320327..ea37885e 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -342,6 +342,10 @@ ao_spi_slave(void); #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; diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 88dc816d..2495a44b 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -194,8 +194,8 @@ ao_flight(void) #if HAS_GPS /* Record current GPS position by waking up GPS log tasks */ - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); + 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)); diff --git a/src/core/ao_gps_report.c b/src/core/ao_gps_report.c index c52ef621..8d15c083 100644 --- a/src/core/ao_gps_report.c +++ b/src/core/ao_gps_report.c @@ -22,78 +22,68 @@ 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 (;;) { - ao_sleep(&ao_gps_data); ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + while ((new = ao_gps_new) == 0) + ao_sleep(&ao_gps_new); + 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 (!(gps_data.flags & AO_GPS_VALID)) - continue; - - 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_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); + } } - } -} - -void -ao_gps_tracking_report(void) -{ - static __xdata struct ao_log_record gps_log; - static __xdata struct ao_telemetry_satellite gps_tracking_data; - uint8_t c, n; - - for (;;) { - ao_sleep(&ao_gps_tracking_data); - ao_mutex_get(&ao_gps_mutex); - gps_log.tick = ao_gps_tick; - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); - ao_mutex_put(&ao_gps_mutex); + if (new & AO_GPS_NEW_TRACKING) { + uint8_t c, n; - if (!(n = gps_tracking_data.channels)) - continue; - - 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); + 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; -__xdata struct ao_task ao_gps_tracking_report_task; void ao_gps_report_init(void) { ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report"); - ao_add_task(&ao_gps_tracking_report_task, ao_gps_tracking_report, "gps_tracking_report"); } diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c index e3af4307..e2adbfbc 100644 --- a/src/core/ao_gps_report_mega.c +++ b/src/core/ao_gps_report_mega.c @@ -23,66 +23,55 @@ ao_gps_report_mega(void) { static __xdata struct ao_log_mega gps_log; static __xdata struct ao_telemetry_location gps_data; - uint8_t date_reported = 0; - - for (;;) { - ao_sleep(&ao_gps_data); - ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); - ao_mutex_put(&ao_gps_mutex); - - if (!(gps_data.flags & AO_GPS_VALID)) - continue; - - 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; - ao_log_mega(&gps_log); - } -} - -void -ao_gps_tracking_report_mega(void) -{ - static __xdata struct ao_log_mega gps_log; static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t date_reported = 0; + uint8_t new; uint8_t c, n, i; for (;;) { - ao_sleep(&ao_gps_tracking_data); ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + 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 (!(n = gps_tracking_data.channels)) - continue; + 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.type = AO_LOG_GPS_SAT; - gps_log.tick = ao_gps_tick; - 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++; - } - gps_log.u.gps_sat.channels = i; - ao_log_mega(&gps_log); + 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; + 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++; + } + gps_log.u.gps_sat.channels = i; + ao_log_mega(&gps_log); + } } } __xdata struct ao_task ao_gps_report_mega_task; -__xdata struct ao_task ao_gps_tracking_report_mega_task; void ao_gps_report_mega_init(void) @@ -90,7 +79,4 @@ ao_gps_report_mega_init(void) ao_add_task(&ao_gps_report_mega_task, ao_gps_report_mega, "gps_report"); - ao_add_task(&ao_gps_tracking_report_mega_task, - ao_gps_tracking_report_mega, - "gps_tracking_report"); } diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c index 4fefe223..b82936dd 100644 --- a/src/core/ao_gps_report_metrum.c +++ b/src/core/ao_gps_report_metrum.c @@ -23,80 +23,69 @@ ao_gps_report_metrum(void) { static __xdata struct ao_log_metrum gps_log; static __xdata struct ao_telemetry_location gps_data; - uint8_t date_reported = 0; - - for (;;) { - ao_sleep(&ao_gps_data); - ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); - ao_mutex_put(&ao_gps_mutex); - - if (!(gps_data.flags & AO_GPS_VALID)) - continue; - - 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); - } -} - -void -ao_gps_tracking_report_metrum(void) -{ - static __xdata struct ao_log_metrum gps_log; static __xdata struct ao_telemetry_satellite gps_tracking_data; uint8_t c, n, i, p, valid, packets; uint8_t svid; + uint8_t date_reported = 0; for (;;) { - ao_sleep(&ao_gps_tracking_data); ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + 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 (!(n = gps_tracking_data.channels)) - continue; + 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_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.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++; } - 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); + 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; -__xdata struct ao_task ao_gps_tracking_report_metrum_task; void ao_gps_report_metrum_init(void) @@ -104,7 +93,4 @@ ao_gps_report_metrum_init(void) ao_add_task(&ao_gps_report_metrum_task, ao_gps_report_metrum, "gps_report"); - ao_add_task(&ao_gps_tracking_report_metrum_task, - ao_gps_tracking_report_metrum, - "gps_tracking_report"); } diff --git a/src/drivers/ao_gps_sirf.c b/src/drivers/ao_gps_sirf.c index 91fc948b..d89435b9 100644 --- a/src/drivers/ao_gps_sirf.c +++ b/src/drivers/ao_gps_sirf.c @@ -19,6 +19,7 @@ #include "ao.h" #endif +__xdata uint8_t ao_gps_new; __xdata uint8_t ao_gps_mutex; __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; @@ -422,8 +423,9 @@ ao_gps(void) __reentrant else ao_gps_data.v_error = ao_sirf_data.v_error / 100; #endif + ao_gps_new |= AO_GPS_NEW_DATA; ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); + ao_wakeup(&ao_gps_new); break; case 4: ao_mutex_get(&ao_gps_mutex); @@ -432,8 +434,9 @@ ao_gps(void) __reentrant ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid; ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1; } + ao_gps_new |= AO_GPS_NEW_TRACKING; ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_tracking_data); + ao_wakeup(&ao_gps_new); break; } } diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index 9a9dff75..944a37f9 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -32,6 +32,7 @@ #define ao_gps_set_speed ao_serial1_set_speed #endif +__xdata uint8_t ao_gps_new; __xdata uint8_t ao_gps_mutex; static __data char ao_gps_char; static __data uint8_t ao_gps_cksum; @@ -293,10 +294,11 @@ ao_nmea_gga(void) if (!ao_gps_error) { ao_mutex_get(&ao_gps_mutex); + ao_gps_new |= AO_GPS_NEW_DATA; ao_gps_tick = ao_gps_next_tick; ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data)); ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); + ao_wakeup(&ao_gps_new); } } @@ -352,9 +354,10 @@ ao_nmea_gsv(void) ao_gps_tracking_next.channels = 0; else if (done) { ao_mutex_get(&ao_gps_mutex); + ao_gps_new |= AO_GPS_NEW_TRACKING; ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data)); ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_tracking_data); + ao_wakeup(&ao_gps_new); } } diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index e9168348..3582d6e0 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -25,6 +25,7 @@ #include +__xdata uint8_t ao_gps_new; __xdata uint8_t ao_gps_mutex; __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; @@ -760,8 +761,8 @@ ao_gps(void) __reentrant } ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); break; } break; diff --git a/src/product/ao_terraui.c b/src/product/ao_terraui.c index 1866eb0c..8fd97033 100644 --- a/src/product/ao_terraui.c +++ b/src/product/ao_terraui.c @@ -629,7 +629,7 @@ ao_terragps(void) for (;;) { while (ao_gps_tick == gps_tick) - ao_sleep(&ao_gps_data); + ao_sleep(&ao_gps_new); gps_tick = ao_gps_tick; ao_gps_progress = (ao_gps_progress + 1) & 3; } diff --git a/src/teleballoon-v1.1/ao_balloon.c b/src/teleballoon-v1.1/ao_balloon.c index e8972655..12752d1f 100644 --- a/src/teleballoon-v1.1/ao_balloon.c +++ b/src/teleballoon-v1.1/ao_balloon.c @@ -105,8 +105,8 @@ ao_flight(void) #if HAS_GPS /* Record current GPS position by waking up GPS log tasks */ - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); + 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)); diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c index 3844a326..b6cc9ba7 100644 --- a/src/test/ao_gps_test.c +++ b/src/test/ao_gps_test.c @@ -427,11 +427,18 @@ void ao_dump_state(void *wchan) { int i; - if (wchan == &ao_gps_data) + + if (wchan != &ao_gps_new) + return; + + if (ao_gps_new & AO_GPS_NEW_DATA) { ao_gps_print(&ao_gps_data); - else + putchar('\n'); + } + if (ao_gps_new & AO_GPS_NEW_TRACKING) { ao_gps_tracking_print(&ao_gps_tracking_data); - putchar('\n'); + putchar('\n'); + } return; printf ("%02d:%02d:%02d", ao_gps_data.hour, ao_gps_data.minute, diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c index 89cbd767..bf2ab5b8 100644 --- a/src/test/ao_gps_test_skytraq.c +++ b/src/test/ao_gps_test_skytraq.c @@ -443,7 +443,7 @@ uint8_t ao_task_minimize_latency; void ao_dump_state(void *wchan) { - if (wchan == &ao_gps_data) + if (wchan == &ao_gps_new) ao_gps_show(); } diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c index a0e04cb6..31c7af60 100644 --- a/src/test/ao_gps_test_ublox.c +++ b/src/test/ao_gps_test_ublox.c @@ -347,7 +347,7 @@ check_ublox_message(char *which, uint8_t *msg) void ao_dump_state(void *wchan) { - if (wchan == &ao_gps_data) + if (wchan == &ao_gps_new) ao_gps_show(); return; } -- cgit v1.2.3 From 08143a922fe27bc50a19924f46538f9476ab5fd1 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 25 Oct 2013 04:05:09 -0700 Subject: altos: Add gyro-based orientation tracking This tracks the angle-from-vertical as an additional input to the pyro channels. Signed-off-by: Keith Packard --- src/core/ao_data.h | 6 +- src/core/ao_flight.c | 4 + src/core/ao_kalman.c | 3 - src/core/ao_pyro.c | 4 +- src/core/ao_quaternion.h | 116 +++++++++++++++++++ src/core/ao_sample.c | 98 +++++++++++++++- src/core/ao_sample.h | 3 +- src/core/ao_telemetry.c | 1 + src/core/ao_telemetry.h | 2 +- src/drivers/ao_mpu6000.c | 2 + src/drivers/ao_mpu6000.h | 17 ++- src/stm/Makefile.defs | 4 +- src/telemega-v0.3/Makefile | 16 +++ src/test/Makefile | 9 +- src/test/ao_flight_test.c | 259 ++---------------------------------------- src/test/ao_quaternion_test.c | 67 +++++++++++ 16 files changed, 343 insertions(+), 268 deletions(-) create mode 100644 src/core/ao_quaternion.h create mode 100644 src/test/ao_quaternion_test.c (limited to 'src/core') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 339afe69..5a232885 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -18,6 +18,8 @@ #ifndef _AO_DATA_H_ #define _AO_DATA_H_ +#define GRAVITY 9.80665 + #if HAS_ADC #define AO_DATA_ADC (1 << 0) #else @@ -300,8 +302,8 @@ typedef int16_t accel_t; #define HAS_GYRO 1 -typedef int16_t gyro_t; -typedef int32_t angle_t; +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) */ diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 2495a44b..240b348a 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -20,6 +20,10 @@ #include #endif +#if HAS_MPU6000 +#include +#endif + #ifndef HAS_ACCEL #error Please define HAS_ACCEL #endif diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c index 762b2c0a..7fd4f889 100644 --- a/src/core/ao_kalman.c +++ b/src/core/ao_kalman.c @@ -292,7 +292,4 @@ ao_kalman(void) else #endif ao_avg_height = (ao_avg_height_scaled + 63) >> 7; -#ifdef AO_FLIGHT_TEST - ao_sample_prev_tick = ao_sample_tick; -#endif } diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index 531e1f50..24c9fe99 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -113,7 +113,7 @@ ao_pyro_ready(struct ao_pyro *pyro) continue; break; -#if HAS_ORIENT +#if HAS_GYRO case ao_pyro_orient_less: if (ao_orient <= pyro->orient_less) continue; @@ -310,7 +310,7 @@ const struct { { "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_ORIENT +#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 diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h new file mode 100644 index 00000000..f4b8aaa4 --- /dev/null +++ b/src/core/ao_quaternion.h @@ -0,0 +1,116 @@ +/* + * 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, + struct ao_quaternion *a, + struct ao_quaternion *b) +{ + struct ao_quaternion t; +#define T(_a,_b) (((a)->_a) * ((b)->_b)) + 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, + 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(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, + 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, + 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 void ao_quaternion_rotate(struct ao_quaternion *r, + struct ao_quaternion *a, + struct ao_quaternion *b) +{ + struct ao_quaternion c; + struct ao_quaternion t; + + ao_quaternion_conjugate(&c, b); + ao_quaternion_multiply(&t, b, a); + ao_quaternion_multiply(r, &t, &c); +} + +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; +} + +#endif /* _AO_QUATERNION_H_ */ diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index dec44f9f..676e0ffd 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -20,6 +20,10 @@ #include #endif +#if HAS_GYRO +#include +#endif + /* * Current sensor values */ @@ -44,8 +48,7 @@ __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; +__pdata angle_t ao_orient; #endif __data uint8_t ao_sample_data; @@ -86,6 +89,8 @@ __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; +static struct ao_quaternion ao_pad_orientation; #endif static void @@ -129,11 +134,91 @@ ao_sample_preflight_set(void) ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_sample_angle = 0; + ao_orient = 0; + + /* No rotation yet */ + ao_quaternion_init_zero_rotation(&ao_rotation); + + /* XXX Assume we're pointing straight up for now */ + ao_quaternion_init_vector(&ao_pad_orientation, + ao_ground_accel_across, + ao_ground_accel_through, + -ao_ground_accel_along); + ao_quaternion_normalize(&ao_pad_orientation, + &ao_pad_orientation); + + printf ("pad r%8.5f x%8.5f y%8.5f z%8.5f\n", + ao_pad_orientation.r, + ao_pad_orientation.x, + ao_pad_orientation.y, + ao_pad_orientation.z); #endif nsamples = 0; } +#if HAS_GYRO +static void +ao_sample_rotate(void) +{ +#ifdef AO_FLIGHT_TEST + float dt = (ao_sample_tick - ao_sample_prev_tick) / 100.0; +#else + static const float dt = 1/100.0; +#endif + float x = ao_mpu6000_gyro(ao_sample_pitch - ao_ground_pitch) * dt; + float y = ao_mpu6000_gyro(ao_sample_yaw - ao_ground_yaw) * dt; + float z = ao_mpu6000_gyro(ao_sample_roll - ao_ground_roll) * dt; + + float n_2, n; + float s, c; + + struct ao_quaternion rot; + struct ao_quaternion point; + + /* The amount of rotation is just the length of the vector. Now, + * here's the trick -- assume that the rotation amount is small. In this case, + * sin(x) ≃ x, so we can just make this the sin. + */ + + n_2 = x*x + y*y + z*z; + n = sqrtf(n_2); + s = n / 2; + if (s > 1) + s = 1; + c = sqrtf(1 - s*s); + + /* Make unit vector */ + if (n > 0) { + x /= n; + y /= n; + z /= n; + } + + /* Now compute the unified rotation quaternion */ + + ao_quaternion_init_rotation(&rot, + x, y, z, + s, c); + + /* Integrate with the previous rotation amount */ + ao_quaternion_multiply(&ao_rotation, &ao_rotation, &rot); + + /* 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 + */ + + ao_quaternion_rotate(&point, &ao_pad_orientation, &ao_rotation); + + ao_orient = acosf(point.z) * (float) (180.0/M_PI); +} +#endif + static void ao_sample_preflight(void) { @@ -232,9 +317,12 @@ ao_sample(void) ao_sample_preflight_update(); ao_kalman(); #if HAS_GYRO - /* do quaternion stuff here... */ + 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; @@ -264,7 +352,7 @@ ao_sample_init(void) ao_sample_pitch = 0; ao_sample_yaw = 0; ao_sample_roll = 0; - ao_sample_angle = 0; + ao_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 index 5bd29536..1d1bcc7c 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -64,8 +64,6 @@ * for all further flight computations */ -#define GRAVITY 9.80665 - /* * Above this height, the baro sensor doesn't work */ @@ -118,6 +116,7 @@ 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; +extern __pdata angle_t ao_orient; #endif void ao_sample_init(void); diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 6b47a06a..a2726016 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -115,6 +115,7 @@ ao_send_mega_sensor(void) telemetry.generic.tick = packet->tick; telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; + telemetry.mega_sensor.orient = ao_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); diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h index fd6b76b3..237a35ab 100644 --- a/src/core/ao_telemetry.h +++ b/src/core/ao_telemetry.h @@ -162,7 +162,7 @@ struct ao_telemetry_mega_sensor { uint16_t tick; /* 2 */ uint8_t type; /* 4 */ - uint8_t pad5; /* 5 */ + uint8_t orient; /* 5 angle from vertical */ int16_t accel; /* 6 Z axis */ int32_t pres; /* 8 Pa * 10 */ diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index 73057820..5e75b78a 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -118,6 +118,7 @@ _ao_mpu6000_sample(struct ao_mpu6000_sample *sample) #define G 981 /* in cm/s² */ +#if 0 static int16_t /* cm/s² */ ao_mpu6000_accel(int16_t v) { @@ -129,6 +130,7 @@ ao_mpu6000_gyro(int16_t v) { return (int16_t) ((v * (int32_t) 20000) / 32767); } +#endif static uint8_t ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which) diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index a42f69c4..2241bf80 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -18,6 +18,10 @@ #ifndef _AO_MPU6000_H_ #define _AO_MPU6000_H_ +#ifndef M_PI +#define M_PI 3.1415926535897832384626433 +#endif + #define MPU6000_ADDR_WRITE 0xd0 #define MPU6000_ADDR_READ 0xd1 @@ -166,9 +170,20 @@ /* Self test gyro is approximately 50°/s */ #define MPU6000_ST_GYRO(full_scale) ((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale))) -#define MPU6000_GYRO_FULLSCALE 2000 +#define MPU6000_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0) + +static inline float +ao_mpu6000_gyro(int16_t sensor) { + return (float) sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0)); +} + #define MPU6000_ACCEL_FULLSCALE 16 +static inline float +ao_mpu6000_accel(int16_t sensor) { + return (float) sensor * ((float) (MPU6000_ACCEL_FULLSCALE * GRAVITY / 32767.0)); +} + struct ao_mpu6000_sample { int16_t accel_x; int16_t accel_y; diff --git a/src/stm/Makefile.defs b/src/stm/Makefile.defs index ee1cb658..0710d747 100644 --- a/src/stm/Makefile.defs +++ b/src/stm/Makefile.defs @@ -1,4 +1,4 @@ -vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:.. +vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:../math:.. vpath make-altitude ../util vpath make-kalman ../util vpath kalman.5c ../kalman @@ -24,7 +24,7 @@ include $(TOPDIR)/Makedefs CC=$(ARM_CC) LIBS=-lpdclib-cortex-m3 -lgcc -AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I.. +AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I../math -I.. STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) LDFLAGS=-L../stm -Wl,-Taltos.ld diff --git a/src/telemega-v0.3/Makefile b/src/telemega-v0.3/Makefile index 6e5da721..2f460105 100644 --- a/src/telemega-v0.3/Makefile +++ b/src/telemega-v0.3/Makefile @@ -27,6 +27,7 @@ INC = \ ao_sample_profile.h \ ao_mpu.h \ stm32l.h \ + math.h \ Makefile # @@ -44,6 +45,20 @@ INC = \ #STACK_GUARD=ao_mpu_stm.c #STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 +MATH_SRC=\ + ef_acos.c \ + ef_sqrt.c + +# ef_rem_pio2.c \ +# kf_cos.c \ +# kf_sin.c \ +# kf_rem_pio2.c \ +# sf_copysign.c \ +# sf_cos.c \ +# sf_fabs.c \ +# sf_floor.c \ +# sf_scalbn.c + ALTOS_SRC = \ ao_boot_chain.c \ ao_interrupt.c \ @@ -93,6 +108,7 @@ ALTOS_SRC = \ ao_companion.c \ ao_pyro.c \ ao_aprs.c \ + $(MATH_SRC) \ $(PROFILE) \ $(SAMPLE_PROFILE) \ $(STACK_GUARD) diff --git a/src/test/Makefile b/src/test/Makefile index 5eee6bbb..f64a9531 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,15 +1,15 @@ -vpath % ..:../core:../drivers:../util:../micropeak:../aes +vpath % ..:../core:../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 \ ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \ - ao_ms5607_convert_test + ao_ms5607_convert_test ao_quaternion_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall +CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -I../product -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -80,3 +80,6 @@ ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c + +ao_quaternion_test: ao_quaternion_test.c ao_quaternion.h + cc $(CFLAGS) -o $@ ao_quaternion_test.c -lm \ No newline at end of file diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index faf31aa7..e2f63e34 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -25,6 +25,8 @@ #include #include +#define GRAVITY 9.80665 + #define AO_HERTZ 100 #define HAS_ADC 1 @@ -36,6 +38,11 @@ #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +int ao_gps_new; + #if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 @@ -240,7 +247,6 @@ struct ao_config ao_config; #define DATA_TO_XDATA(x) (x) -#define GRAVITY 9.80665 extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; @@ -339,20 +345,6 @@ ao_test_exit(void) exit(0); } -#if HAS_MPU6000 -static double -ao_mpu6000_accel(int16_t sensor) -{ - return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY; -} - -static double -ao_mpu6000_gyro(int32_t sensor) -{ - return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; -} -#endif - void ao_insert(void) { @@ -408,23 +400,23 @@ ao_insert(void) if (!ao_summary) { printf("%7.2f height %8.2f accel %8.3f " #if TELEMEGA - "roll %8.3f angle %8.3f qangle %8.3f " - "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " + "angle %5d " +/* "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " */ #endif "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", time, height, accel, #if TELEMEGA - ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0, - ao_mpu6000_gyro(ao_sample_angle) / 100.0, - ao_sample_qangle, + ao_orient, +/* ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), ao_mpu6000_accel(ao_data_static.mpu6000.accel_z), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), +*/ #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -589,207 +581,6 @@ int32(uint8_t *bytes, int off) static int log_format; -#if TELEMEGA - -static double -ao_vec_norm(double x, double y, double z) -{ - return x*x + y*y + z*z; -} - -static void -ao_vec_normalize(double *x, double *y, double *z) -{ - double scale = 1/sqrt(ao_vec_norm(*x, *y, *z)); - - *x *= scale; - *y *= scale; - *z *= scale; -} - -struct ao_quat { - double q0, q1, q2, q3; -}; - -static void -ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b) -{ - r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3; - r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2; - r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1; - r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0; -} - -#if 0 -static void -ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s) -{ - r->q0 = a->q0 * s; - r->q1 = a->q1 * s; - r->q2 = a->q2 * s; - r->q3 = a->q3 * s; -} -#endif - -static void -ao_quat_conj(struct ao_quat *r, struct ao_quat *a) -{ - r->q0 = a->q0; - r->q1 = -a->q1; - r->q2 = -a->q2; - r->q3 = -a->q3; -} - -static void -ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q) -{ - struct ao_quat t; - struct ao_quat c; - ao_quat_mul(&t, q, a); - ao_quat_conj(&c, q); - ao_quat_mul(r, &t, &c); -} - -static void -ao_quat_from_angle(struct ao_quat *r, - double x_rad, - double y_rad, - double z_rad) -{ - double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad); - double s = sin(angle/2); - double c = cos(angle/2); - - r->q0 = c; - r->q1 = x_rad * s / angle; - r->q2 = y_rad * s / angle; - r->q3 = z_rad * s / angle; -} - -static void -ao_quat_from_vector(struct ao_quat *r, double x, double y, double z) -{ - ao_vec_normalize(&x, &y, &z); - double x_rad = atan2(z, y); - double y_rad = atan2(x, z); - double z_rad = atan2(y, x); - - ao_quat_from_angle(r, x_rad, y_rad, z_rad); -} - -static double -ao_quat_norm(struct ao_quat *a) -{ - return (a->q0 * a->q0 + - a->q1 * a->q1 + - a->q2 * a->q2 + - a->q3 * a->q3); -} - -static void -ao_quat_normalize(struct ao_quat *a) -{ - double norm = ao_quat_norm(a); - - if (norm) { - double m = 1/sqrt(norm); - - a->q0 *= m; - a->q1 *= m; - a->q2 *= m; - a->q3 *= m; - } -} - -static struct ao_quat ao_up, ao_current; -static struct ao_quat ao_orient; -static int ao_orient_tick; - -void -set_orientation(double x, double y, double z, int tick) -{ - struct ao_quat t; - - printf ("set_orientation %g %g %g\n", x, y, z); - ao_quat_from_vector(&ao_orient, x, y, z); - ao_up.q1 = ao_up.q2 = 0; - ao_up.q0 = ao_up.q3 = sqrt(2)/2; - ao_orient_tick = tick; - - ao_orient.q0 = 1; - ao_orient.q1 = 0; - ao_orient.q2 = 0; - ao_orient.q3 = 0; - - printf ("orient (%g) %g %g %g up (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_up.q0, - ao_up.q1, - ao_up.q2, - ao_up.q3); - - ao_quat_rot(&t, &ao_up, &ao_orient); - printf ("pad orient (%g) %g %g %g\n", - t.q0, - t.q1, - t.q2, - t.q3); - -} - -void -update_orientation (double rate_x, double rate_y, double rate_z, int tick) -{ - struct ao_quat q_dot; - double lambda; - double dt = (tick - ao_orient_tick) / 100.0; - - ao_orient_tick = tick; - -// lambda = 1 - ao_quat_norm(&ao_orient); - lambda = 0; - - q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0; - q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1; - q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2; - q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3; - -#if 0 - printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt); - printf ("q_dot (%g) %g %g %g\n", - q_dot.q0, - q_dot.q1, - q_dot.q2, - q_dot.q3); -#endif - - ao_orient.q0 += q_dot.q0 * dt; - ao_orient.q1 += q_dot.q1 * dt; - ao_orient.q2 += q_dot.q2 * dt; - ao_orient.q3 += q_dot.q3 * dt; - - ao_quat_normalize(&ao_orient); - - ao_quat_rot(&ao_current, &ao_up, &ao_orient); - - ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2)); -#if 0 - printf ("orient (%g) %g %g %g current (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_current.q0, - ao_current.q1, - ao_current.q2, - ao_current.q3); -#endif -} -#endif - void ao_sleep(void *wchan) { @@ -872,32 +663,6 @@ ao_sleep(void *wchan) #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif - if (ao_records_read == 0) - ao_ground_mpu6000 = ao_data_static.mpu6000; - else if (ao_records_read < 10) { -#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2) - f(accel_x); - f(accel_y); - f(accel_z); - f(gyro_x); - f(gyro_y); - f(gyro_z); - - double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x); - double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y); - double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z); - - /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes - * Z is normal to the ground, the MPU y axis - */ - set_orientation(accel_x, accel_z, accel_y, tick); - } else { - double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x); - double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y); - double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z); - - update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick); - } ao_records_read++; ao_insert(); return; diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c new file mode 100644 index 00000000..0e4b5b07 --- /dev/null +++ b/src/test/ao_quaternion_test.c @@ -0,0 +1,67 @@ +/* + * 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. + */ + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include + +#include "ao_quaternion.h" + +static void +print_q(char *name, struct ao_quaternion *q) +{ + printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name, + q->r, q->x, q->y, q->z); +} + +int main(int argc, char **argv) +{ + struct ao_quaternion position; + struct ao_quaternion rotation; + struct ao_quaternion little_rotation; + int i; + + /* unit x vector */ + ao_quaternion_init_vector(&position, 1, 0, 0); + + /* zero rotation */ + ao_quaternion_init_zero_rotation(&rotation); + + /* π/16 rotation around Z axis */ + ao_quaternion_init_rotation(&little_rotation, 0, 0, 1, + sin((M_PI/16)/2), + cos((M_PI/16)/2)); + for (i = 0; i <= 16; i++) { + struct ao_quaternion rotated; + + ao_quaternion_rotate(&rotated, &position, &rotation); + print_q("position", &position); + print_q("rotated", &rotated); + print_q("rotation", &rotation); + printf ("\n"); + ao_quaternion_multiply(&rotation, &rotation, &little_rotation); + ao_quaternion_normalize(&rotation, &rotation); + } + return 0; +} + -- cgit v1.2.3 From 351d53836e201834a2d89773a08ab7c2dab2b2f4 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 25 Oct 2013 04:34:16 -0700 Subject: altos: Calibrate IMU accelerometers too Average the IMU accelerometer values pointing up and down so that we have a zero-g offset for all three axes. This can then be used to compute which direction the rocket is pointing while sitting on the pad. Signed-off-by: Keith Packard --- src/core/ao.h | 7 +++++- src/core/ao_config.c | 61 +++++++++++++++++++++++++++++++++++++++++++++++ src/core/ao_sample.c | 15 +++++------- src/test/ao_flight_test.c | 10 ++++++++ 4 files changed, 83 insertions(+), 10 deletions(-) (limited to 'src/core') diff --git a/src/core/ao.h b/src/core/ao.h index ea37885e..d12f13a0 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -739,7 +739,7 @@ extern __xdata uint8_t ao_force_freq; #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 14 +#define AO_CONFIG_MINOR 15 #define AO_AES_LEN 16 @@ -773,6 +773,11 @@ struct ao_config { #if HAS_RADIO_AMP uint8_t radio_amp; /* minor version 14 */ #endif +#if HAS_GYRO + uint16_t accel_zero_along; /* minor version 15 */ + uint16_t accel_zero_across; /* minor version 15 */ + uint16_t accel_zero_through; /* minor version 15 */ +#endif }; #define AO_IGNITE_MODE_DUAL 0 diff --git a/src/core/ao_config.c b/src/core/ao_config.c index b480e14c..82faf32b 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -155,6 +155,19 @@ _ao_config_get(void) #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; @@ -275,17 +288,34 @@ 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(); @@ -299,10 +329,20 @@ ao_config_accel_calibrate_auto(char *orientation) __reentrant 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; } @@ -310,12 +350,28 @@ void ao_config_accel_calibrate_set(void) __reentrant { int16_t up, down; +#if HAS_GYRO + int16_t accel_along_up, accel_along_down; + int16_t accel_across_up, accel_across_down; + int16_t accel_through_up, accel_through_down; +#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(); @@ -331,6 +387,11 @@ ao_config_accel_calibrate_set(void) __reentrant _ao_config_edit_start(); ao_config.accel_plus_g = up; ao_config.accel_minus_g = down; +#if HAS_GYRO + 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 */ diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index 676e0ffd..a9d50cb2 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -139,19 +139,16 @@ ao_sample_preflight_set(void) /* No rotation yet */ ao_quaternion_init_zero_rotation(&ao_rotation); - /* XXX Assume we're pointing straight up for now */ + /* Take the pad IMU acceleration values and compute our current direction + */ ao_quaternion_init_vector(&ao_pad_orientation, - ao_ground_accel_across, - ao_ground_accel_through, - -ao_ground_accel_along); + 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(&ao_pad_orientation, &ao_pad_orientation); - printf ("pad r%8.5f x%8.5f y%8.5f z%8.5f\n", - ao_pad_orientation.r, - ao_pad_orientation.x, - ao_pad_orientation.y, - ao_pad_orientation.z); #endif nsamples = 0; } diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index e2f63e34..7f18c80e 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -234,6 +234,9 @@ struct ao_config { uint16_t apogee_lockout; #if TELEMEGA struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ + int16_t accel_zero_along; + int16_t accel_zero_across; + int16_t accel_zero_through; #endif }; @@ -719,6 +722,13 @@ ao_sleep(void *wchan) } else if (nword >= 6 && strcmp(words[0], "Accel") == 0) { ao_config.accel_plus_g = atoi(words[3]); ao_config.accel_minus_g = atoi(words[5]); +#ifdef TELEMEGA + } else if (nword >= 8 && strcmp(words[0], "IMU") == 0) { + ao_config.accel_zero_along = atoi(words[3]); + ao_config.accel_zero_across = atoi(words[5]); + ao_config.accel_zero_through = atoi(words[7]); + printf ("%d %d %d\n", ao_config.accel_zero_along, ao_config.accel_zero_across, ao_config.accel_zero_through); +#endif } else if (nword >= 4 && strcmp(words[0], "Main") == 0) { ao_config.main_deploy = atoi(words[2]); } else if (nword >= 3 && strcmp(words[0], "Apogee") == 0 && -- cgit v1.2.3 From e923e11e185fd42d2a83e18b3d13bd839a72b1aa Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 22:44:49 -0700 Subject: altos: IMU accel calibration values need to be signed The MPU6000 reports signed values. Signed-off-by: Keith Packard --- src/core/ao.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/core') diff --git a/src/core/ao.h b/src/core/ao.h index d12f13a0..0b634a79 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -774,9 +774,9 @@ struct ao_config { uint8_t radio_amp; /* minor version 14 */ #endif #if HAS_GYRO - uint16_t accel_zero_along; /* minor version 15 */ - uint16_t accel_zero_across; /* minor version 15 */ - uint16_t accel_zero_through; /* minor version 15 */ + 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 }; -- cgit v1.2.3 From 616977d2955da13383a1869b9ccdb07338172109 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:10:13 -0700 Subject: altos: Mark arguments to quaternion functions as const Lets us pass constants without the compile whinging Signed-off-by: Keith Packard --- src/core/ao_quaternion.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h index f4b8aaa4..1c0617c4 100644 --- a/src/core/ao_quaternion.h +++ b/src/core/ao_quaternion.h @@ -26,8 +26,8 @@ struct ao_quaternion { }; static inline void ao_quaternion_multiply(struct ao_quaternion *r, - struct ao_quaternion *a, - struct ao_quaternion *b) + const struct ao_quaternion *a, + const struct ao_quaternion *b) { struct ao_quaternion t; #define T(_a,_b) (((a)->_a) * ((b)->_b)) @@ -40,7 +40,7 @@ static inline void ao_quaternion_multiply(struct ao_quaternion *r, } static inline void ao_quaternion_conjugate(struct ao_quaternion *r, - struct ao_quaternion *a) + const struct ao_quaternion *a) { r->r = a->r; r->x = -a->x; @@ -48,7 +48,7 @@ static inline void ao_quaternion_conjugate(struct ao_quaternion *r, r->z = -a->z; } -static inline float ao_quaternion_normal(struct ao_quaternion *a) +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); @@ -56,7 +56,7 @@ static inline float ao_quaternion_normal(struct ao_quaternion *a) } static inline void ao_quaternion_scale(struct ao_quaternion *r, - struct ao_quaternion *a, + const struct ao_quaternion *a, float b) { r->r = a->r * b; @@ -66,7 +66,7 @@ static inline void ao_quaternion_scale(struct ao_quaternion *r, } static inline void ao_quaternion_normalize(struct ao_quaternion *r, - struct ao_quaternion *a) + const struct ao_quaternion *a) { float n = ao_quaternion_normal(a); -- cgit v1.2.3 From 3b25860b5b3b69642928dd9c30dec4b4b937a88c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:11:09 -0700 Subject: altos: Add some comments describing quaternion multiplication Signed-off-by: Keith Packard --- src/core/ao_quaternion.h | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) (limited to 'src/core') diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h index 1c0617c4..6a701a18 100644 --- a/src/core/ao_quaternion.h +++ b/src/core/ao_quaternion.h @@ -31,6 +31,39 @@ static inline void ao_quaternion_multiply(struct ao_quaternion *r, { 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); -- cgit v1.2.3 From c10cb9d31765e6ef0ba737bc484c5aed22a332f9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:11:37 -0700 Subject: altos: Add functions to init quaternions from vector pairs and euler angles MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Our low sampling rate means that the "cheap" hack for integrating quaternion rotations by using sin(x) ≃ x doesn't work, so instead we have to compute the partial rotation the hard way. Signed-off-by: Keith Packard --- src/core/ao_quaternion.h | 97 ++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 94 insertions(+), 3 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h index 6a701a18..6c885500 100644 --- a/src/core/ao_quaternion.h +++ b/src/core/ao_quaternion.h @@ -110,17 +110,68 @@ static inline void ao_quaternion_normalize(struct ao_quaternion *r, } static inline void ao_quaternion_rotate(struct ao_quaternion *r, - struct ao_quaternion *a, - struct ao_quaternion *b) + const struct ao_quaternion *a, + const struct ao_quaternion *b) { struct ao_quaternion c; struct ao_quaternion t; - ao_quaternion_conjugate(&c, b); 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) { @@ -146,4 +197,44 @@ static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r) 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_ */ -- cgit v1.2.3 From 58f08c4b3cb9049d0c9cb02cde0d8dbdc3d33920 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:23:59 -0700 Subject: altos: Rename ao_orient to ao_sample_orient Keeps it clear where this name comes from. Signed-off-by: Keith Packard --- src/core/ao_pyro.c | 4 ++-- src/core/ao_sample.c | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index 24c9fe99..a260aa99 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -115,11 +115,11 @@ ao_pyro_ready(struct ao_pyro *pyro) #if HAS_GYRO case ao_pyro_orient_less: - if (ao_orient <= pyro->orient_less) + if (ao_sample_orient <= pyro->orient_less) continue; break; case ao_pyro_orient_greater: - if (ao_orient >= pyro->orient_greater) + if (ao_sample_orient >= pyro->orient_greater) continue; break; #endif diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index a9d50cb2..47c5ea2e 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -48,7 +48,7 @@ __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_orient; +__pdata angle_t ao_sample_orient; #endif __data uint8_t ao_sample_data; @@ -134,7 +134,7 @@ ao_sample_preflight_set(void) ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_orient = 0; + ao_sample_orient = 0; /* No rotation yet */ ao_quaternion_init_zero_rotation(&ao_rotation); @@ -212,7 +212,7 @@ ao_sample_rotate(void) ao_quaternion_rotate(&point, &ao_pad_orientation, &ao_rotation); - ao_orient = acosf(point.z) * (float) (180.0/M_PI); + ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI); } #endif @@ -349,7 +349,7 @@ ao_sample_init(void) ao_sample_pitch = 0; ao_sample_yaw = 0; ao_sample_roll = 0; - ao_orient = 0; + ao_sample_orient = 0; #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; -- cgit v1.2.3 From fa7d0ba0efdde3ac9fb4df0589f9ead07b7ffff5 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:26:28 -0700 Subject: altos: Keep 9 more bits of average pad IMU gyro data This reduces the offset error by a bit, minimizing gyro drift. Signed-off-by: Keith Packard --- src/core/ao_sample.c | 22 +++++++++------------- 1 file changed, 9 insertions(+), 13 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index 47c5ea2e..b425aee7 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -70,9 +70,9 @@ __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ __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; +__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 */ @@ -125,9 +125,9 @@ ao_sample_preflight_set(void) 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_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; @@ -162,13 +162,9 @@ ao_sample_rotate(void) #else static const float dt = 1/100.0; #endif - float x = ao_mpu6000_gyro(ao_sample_pitch - ao_ground_pitch) * dt; - float y = ao_mpu6000_gyro(ao_sample_yaw - ao_ground_yaw) * dt; - float z = ao_mpu6000_gyro(ao_sample_roll - ao_ground_roll) * dt; - - float n_2, n; - float s, c; - + 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; struct ao_quaternion point; -- cgit v1.2.3 From cdbe8ce33e4a75e85caf07538ed7e997f462b758 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:33:11 -0700 Subject: altos: Fixup for ao_sample_orient rename Signed-off-by: Keith Packard --- src/core/ao_telemetry.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index a2726016..c118d007 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -115,7 +115,7 @@ ao_send_mega_sensor(void) telemetry.generic.tick = packet->tick; telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; - telemetry.mega_sensor.orient = ao_orient; + 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); -- cgit v1.2.3 From 06b0c1b768a7d3eae57e66bc9aea25db49f9ea8a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:35:54 -0700 Subject: altos: Compute initial rotation from vertical This initializes the rotation with the angle from vertical, rather than simply recording the off-angle vector. Doing this allows us to accurately track the true orientation of the rocket, instead of just the offset from the initial non-vertical orientation. Signed-off-by: Keith Packard --- src/core/ao_sample.c | 50 +++++++++++++++++++++++++++++++++++++------------- 1 file changed, 37 insertions(+), 13 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index b425aee7..fc8f8680 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -90,7 +90,6 @@ __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; -static struct ao_quaternion ao_pad_orientation; #endif static void @@ -136,19 +135,30 @@ ao_sample_preflight_set(void) ao_sample_roll_sum = 0; ao_sample_orient = 0; - /* No rotation yet */ - ao_quaternion_init_zero_rotation(&ao_rotation); + struct ao_quaternion orient; /* Take the pad IMU acceleration values and compute our current direction */ - ao_quaternion_init_vector(&ao_pad_orientation, - 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(&ao_pad_orientation, - &ao_pad_orientation); - + + 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); #endif nsamples = 0; } @@ -203,10 +213,24 @@ ao_sample_rotate(void) * 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 + * 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² */ - ao_quaternion_rotate(&point, &ao_pad_orientation, &ao_rotation); + 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); } -- cgit v1.2.3 From 4bebade9e9004bad81df1a423687f3e3f356f1c2 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:37:55 -0700 Subject: altos: Correct incremental rotation computation Trying to compute the combined rotation by taking the x/y/z rotations as a vector is a good approximation, but not accurate enough for our application given the large angles we sometimes see. Instead, use a correct-but-expensive function with a pile of transcendental function calls. The STM32L seems to be fast enough at least... Signed-off-by: Keith Packard --- src/core/ao_sample.c | 37 +++++++------------------------------ 1 file changed, 7 insertions(+), 30 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index fc8f8680..adf8399d 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -164,47 +164,24 @@ ao_sample_preflight_set(void) } #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) / 100.0; + float dt = (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; #else - static const float dt = 1/100.0; + 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; - struct ao_quaternion point; - - /* The amount of rotation is just the length of the vector. Now, - * here's the trick -- assume that the rotation amount is small. In this case, - * sin(x) ≃ x, so we can just make this the sin. - */ - - n_2 = x*x + y*y + z*z; - n = sqrtf(n_2); - s = n / 2; - if (s > 1) - s = 1; - c = sqrtf(1 - s*s); - - /* Make unit vector */ - if (n > 0) { - x /= n; - y /= n; - z /= n; - } - - /* Now compute the unified rotation quaternion */ - - ao_quaternion_init_rotation(&rot, - x, y, z, - s, c); - /* Integrate with the previous rotation amount */ - ao_quaternion_multiply(&ao_rotation, &ao_rotation, &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); -- cgit v1.2.3 From 3d3fe7e9b6502432868f4430befac871dfea4869 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:42:26 -0700 Subject: altos: Fixup for 32-bit gyro averages Signed-off-by: Keith Packard --- src/core/ao_sample.h | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index 1d1bcc7c..16d4c507 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -113,10 +113,16 @@ extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ 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; -extern __pdata angle_t ao_orient; +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); -- cgit v1.2.3 From 9b0ce8ca65d76b9cf55dfff002e13ce2fbb5f7fc Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:45:48 -0700 Subject: altos: Add orientation test when HAS_FLIGHT_DEBUG is set This just dumps the current orientation to stdout so you can monitor it in real time Signed-off-by: Keith Packard --- src/core/ao_flight.c | 21 +++++++++++++++++++++ src/core/ao_flight.h | 3 ++- 2 files changed, 23 insertions(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 240b348a..4a53bdc6 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -364,6 +364,18 @@ ao_flight(void) 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; } @@ -414,8 +426,17 @@ ao_flight_dump(void) 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; +} + __code struct ao_cmds ao_flight_cmds[] = { { ao_flight_dump, "F\0Dump flight status" }, + { ao_gyro_test, "G\0Test gyro code" }, { 0, NULL }, }; #endif diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h index b80202f0..ac3e9cfb 100644 --- a/src/core/ao_flight.h +++ b/src/core/ao_flight.h @@ -33,7 +33,8 @@ enum ao_flight_state { ao_flight_drogue = 6, ao_flight_main = 7, ao_flight_landed = 8, - ao_flight_invalid = 9 + ao_flight_invalid = 9, + ao_flight_test = 10 }; extern __pdata enum ao_flight_state ao_flight_state; -- cgit v1.2.3 From d3628bd2dd3612065792aef6c7ae5bc967b4f081 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 Oct 2013 00:24:59 -0700 Subject: altos: sample profile address range was too narrow The range was cranked down at some point to diagnose issues within the task scheduler. Unfortunately, that change got merged, which meant that general profiling lost information outside of the lower 4kB of code. Signed-off-by: Keith Packard --- src/core/ao_sample_profile.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_sample_profile.c b/src/core/ao_sample_profile.c index e682bd98..d3743d12 100644 --- a/src/core/ao_sample_profile.c +++ b/src/core/ao_sample_profile.c @@ -24,11 +24,11 @@ #endif #ifndef AO_SAMPLE_PROFILE_HIGH_PC -#define AO_SAMPLE_PROFILE_HIGH_PC 0x08003000 +#define AO_SAMPLE_PROFILE_HIGH_PC 0x0800f000 #endif #ifndef AO_SAMPLE_PROFILE_SHIFT -#define AO_SAMPLE_PROFILE_SHIFT 3 +#define AO_SAMPLE_PROFILE_SHIFT 6 #endif #define AO_SAMPLE_PROFILE_RANGE (AO_SAMPLE_PROFILE_HIGH_PC - AO_SAMPLE_PROFILE_LOW_PC) -- cgit v1.2.3 From 29b48b63305881471d9b97ef3fb236af03cb79f5 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 Oct 2013 00:36:13 -0700 Subject: altos: Don't hold GPS mutex while waiting for GPS data in report code Oops. This kinda breaks anyone else waiting for GPS data Signed-off-by: Keith Packard --- src/core/ao_gps_report_mega.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c index e2adbfbc..a66068ab 100644 --- a/src/core/ao_gps_report_mega.c +++ b/src/core/ao_gps_report_mega.c @@ -29,9 +29,9 @@ ao_gps_report_mega(void) uint8_t c, n, i; for (;;) { - ao_mutex_get(&ao_gps_mutex); 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) -- cgit v1.2.3 From bdd6244d8b4a55c9aa4fb79b0cb1a0727afbc2ac Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 12 Nov 2013 14:01:55 +0900 Subject: altos: Add orientation tracking to ao_flight_test Shows calculated offset from vertical in ao_flight_test output Signed-off-by: Keith Packard --- src/core/ao_data.h | 12 +++++ src/core/ao_quaternion.h | 9 ++++ src/test/ao_flight_test.c | 111 +++++++++++++++++++++++++++++++++++++++++++--- 3 files changed, 127 insertions(+), 5 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 5a232885..e1d8a139 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -319,4 +319,16 @@ typedef int16_t angle_t; /* in degrees */ #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_quaternion.h b/src/core/ao_quaternion.h index 6c885500..044f1607 100644 --- a/src/core/ao_quaternion.h +++ b/src/core/ao_quaternion.h @@ -109,6 +109,15 @@ static inline void ao_quaternion_normalize(struct ao_quaternion *r, *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) diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 952a811a..452f5b75 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -48,6 +48,7 @@ int ao_gps_new; #define HAS_MS5607 1 #define HAS_MPU6000 1 #define HAS_MMA655X 1 +#define HAS_HMC5883 1 struct ao_adc { int16_t sense[AO_ADC_NUM_SENSE]; @@ -349,6 +350,23 @@ ao_test_exit(void) exit(0); } +#ifdef TELEMEGA +struct ao_azel { + int az; + int el; +}; + +static void +azel (struct ao_azel *r, struct ao_quaternion *q) +{ + double v; + + r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5); + v = sqrt (q->x*q->x + q->y*q->y); + r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5); +} +#endif + void ao_insert(void) { @@ -402,10 +420,86 @@ ao_insert(void) } if (!ao_summary) { +#if TELEMEGA + static struct ao_quaternion ao_ground_mag; + static int ao_ground_mag_set; + + if (!ao_ground_mag_set) { + ao_quaternion_init_vector (&ao_ground_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag); + ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation); + ao_ground_mag_set = 1; + } + + struct ao_quaternion ao_mag, ao_mag_rot; + + ao_quaternion_init_vector(&ao_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + + ao_quaternion_normalize(&ao_mag, &ao_mag); + ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation); + + float ao_dot; + int ao_mag_angle; + + ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag); + + struct ao_azel ground_azel, mag_azel, rot_azel; + + azel(&ground_azel, &ao_ground_mag); + azel(&mag_azel, &ao_mag); + azel(&rot_azel, &ao_mag_rot); + + ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5); + + static struct ao_quaternion ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 }; + struct ao_quaternion ao_out; + + ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation); + + int out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI); + + printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n", + time, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_sample_orient, out, + mag_azel.el, + mag_azel.az); + + +#if 0 + printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n", + ao_state_names[ao_flight_state], + ground_azel.az, ground_azel.el, + mag_azel.az, mag_azel.el, + rot_azel.az, rot_azel.el, + ground_azel.el - rot_azel.el, + ground_azel.az - rot_azel.az, + ao_mag_angle, + ao_sample_orient, + ao_ground_mag.x, + ao_ground_mag.y, + ao_ground_mag.z, + ao_mag.x, + ao_mag.y, + ao_mag.z, + ao_mag_rot.x, + ao_mag_rot.y, + ao_mag_rot.z); +#endif +#endif + +#if 0 printf("%7.2f height %8.2f accel %8.3f " #if TELEMEGA "angle %5d " -/* "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " */ + "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d " #endif "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", time, @@ -413,14 +507,17 @@ ao_insert(void) accel, #if TELEMEGA ao_sample_orient, -/* + ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), ao_mpu6000_accel(ao_data_static.mpu6000.accel_z), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), -*/ + ao_data_static.hmc5883.x, + ao_data_static.hmc5883.y, + ao_data_static.hmc5883.z, + ao_mag_angle, #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -430,6 +527,7 @@ ao_insert(void) drogue_height, main_height, ao_error_h_sq_avg); +#endif // if (ao_flight_state == ao_flight_landed) // ao_test_exit(); @@ -659,11 +757,14 @@ ao_sleep(void *wchan) ao_data_static.ms5607_raw.temp = int32(bytes, 4); ao_ms5607_convert(&ao_data_static.ms5607_raw, &value); ao_data_static.mpu6000.accel_x = int16(bytes, 8); - ao_data_static.mpu6000.accel_y = -int16(bytes, 10); + ao_data_static.mpu6000.accel_y = int16(bytes, 10); ao_data_static.mpu6000.accel_z = int16(bytes, 12); ao_data_static.mpu6000.gyro_x = int16(bytes, 14); - ao_data_static.mpu6000.gyro_y = -int16(bytes, 16); + ao_data_static.mpu6000.gyro_y = int16(bytes, 16); ao_data_static.mpu6000.gyro_z = int16(bytes, 18); + ao_data_static.hmc5883.x = int16(bytes, 20); + ao_data_static.hmc5883.y = int16(bytes, 22); + ao_data_static.hmc5883.z = int16(bytes, 24); #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif -- cgit v1.2.3 From b57f1cabfe5052306cb4c28793bea477f4aeb2d2 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 12 Nov 2013 15:18:58 +0900 Subject: altos: Don't hold GPS mutex while waiting in TM v2.0 report Holding the GPS mutex while waiting for the GPS code to dump data into the GPS variables is rather counter-productive. Signed-off-by: Keith Packard --- src/core/ao_gps_report_metrum.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c index b82936dd..fa038976 100644 --- a/src/core/ao_gps_report_metrum.c +++ b/src/core/ao_gps_report_metrum.c @@ -27,11 +27,12 @@ ao_gps_report_metrum(void) uint8_t c, n, i, p, valid, packets; uint8_t svid; uint8_t date_reported = 0; + uint8_t new; for (;;) { - ao_mutex_get(&ao_gps_mutex); 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) -- cgit v1.2.3 From 83437b2fe304599e22d0a98b5410808bcb67dc97 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 12 Nov 2013 15:45:32 +0900 Subject: altos: Allow use of internal EEPROM for config storage This stops exposing eeprom as 'storage' and instead exposes it with a separate eeprom API so that it can be used for config storage without also using it for flight log storage. The config code has been changed to allow it to either use storage for configuration data or eeprom. Signed-off-by: Keith Packard --- src/core/ao_config.c | 21 ++++++++-------- src/core/ao_config.h | 57 +++++++++++++++++++++++++++++++++++++++++++ src/core/ao_eeprom.h | 43 ++++++++++++++++++++++++++++++++ src/core/ao_storage.c | 2 +- src/core/ao_storage.h | 10 ++++++++ src/stm/ao_eeprom_stm.c | 57 ++++++++++--------------------------------- src/telegps-v0.1/Makefile | 1 - src/telegps-v0.1/ao_pins.h | 5 +++- src/telegps-v0.1/ao_telegps.c | 3 ++- src/telelco-v0.2/Makefile | 1 - src/telelco-v0.2/ao_pins.h | 2 ++ src/telelco-v0.2/ao_telelco.c | 3 ++- 12 files changed, 145 insertions(+), 60 deletions(-) create mode 100644 src/core/ao_config.h create mode 100644 src/core/ao_eeprom.h (limited to 'src/core') diff --git a/src/core/ao_config.c b/src/core/ao_config.c index 82faf32b..5567587b 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -17,7 +17,7 @@ #include "ao.h" #include "ao_log.h" -#include +#include #if HAS_FLIGHT #include #include @@ -59,13 +59,12 @@ __xdata uint8_t ao_config_mutex; static void _ao_config_put(void) { - ao_storage_setup(); - ao_storage_erase(ao_storage_config); - ao_storage_write(ao_storage_config, &ao_config, sizeof (ao_config)); + ao_config_setup(); + ao_config_write(&ao_config, sizeof (ao_config)); #if HAS_FLIGHT ao_log_write_erase(0); #endif - ao_storage_flush(); + ao_config_flush(); } void @@ -97,8 +96,8 @@ _ao_config_get(void) * but ao_storage_setup *also* sets ao_storage_config, which we * need before calling ao_storage_read here */ - ao_storage_setup(); - ao_storage_read(ao_storage_config, &ao_config, sizeof (ao_config)); + ao_config_setup(); + ao_config_read(&ao_config, sizeof (ao_config)); #endif if (ao_config.major != AO_CONFIG_MAJOR) { ao_config.major = AO_CONFIG_MAJOR; @@ -127,8 +126,10 @@ _ao_config_get(void) 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; @@ -655,7 +656,7 @@ static void ao_config_show(void) __reentrant; static void -ao_config_write(void) __reentrant; +ao_config_save(void) __reentrant; __code struct ao_config_var ao_config_vars[] = { #if HAS_FLIGHT @@ -714,7 +715,7 @@ __code struct ao_config_var ao_config_vars[] = { ao_config_show, 0 }, #if HAS_EEPROM { "w\0Write to eeprom", - ao_config_write, 0 }, + ao_config_save, 0 }, #endif { "?\0Help", ao_config_help, 0 }, @@ -766,7 +767,7 @@ ao_config_show(void) __reentrant #if HAS_EEPROM static void -ao_config_write(void) __reentrant +ao_config_save(void) __reentrant { uint8_t saved = 0; ao_mutex_get(&ao_config_mutex); diff --git a/src/core/ao_config.h b/src/core/ao_config.h new file mode 100644 index 00000000..5e38430e --- /dev/null +++ b/src/core/ao_config.h @@ -0,0 +1,57 @@ +/* + * 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_write(bytes, len) do { \ + ao_storage_erase(ao_storage_config); \ + ao_storage_write(ao_storage_config, bytes, len); \ + } while (0) + +#define ao_config_read(bytes, len) ao_storage_read(ao_storage_config, bytes, len) + +#define ao_config_flush() ao_storage_flush() + +#endif + +#if USE_EEPROM_CONFIG + +#include + +#define ao_config_setup() +#define ao_config_write(bytes, len) ao_eeprom_write(0, bytes, len) +#define ao_config_read(bytes, len) ao_eeprom_read(0, bytes, len) +#define ao_config_flush() + +#endif + +#endif /* _AO_CONFIG_H_ */ diff --git a/src/core/ao_eeprom.h b/src/core/ao_eeprom.h new file mode 100644 index 00000000..915522bf --- /dev/null +++ b/src/core/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/core/ao_storage.c b/src/core/ao_storage.c index adf7e4d4..6eddae7f 100644 --- a/src/core/ao_storage.c +++ b/src/core/ao_storage.c @@ -154,7 +154,7 @@ ao_storage_zapall(void) __reentrant ao_cmd_white(); if (!ao_match_word("DoIt")) return; - for (pos = 0; pos < ao_storage_config; pos += ao_storage_block) + for (pos = 0; pos < ao_storage_log_max; pos += ao_storage_block) ao_storage_erase(pos); } diff --git a/src/core/ao_storage.h b/src/core/ao_storage.h index ea946399..d6e95605 100644 --- a/src/core/ao_storage.h +++ b/src/core/ao_storage.h @@ -35,9 +35,19 @@ 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; diff --git a/src/stm/ao_eeprom_stm.c b/src/stm/ao_eeprom_stm.c index 58783f1a..4207a860 100644 --- a/src/stm/ao_eeprom_stm.c +++ b/src/stm/ao_eeprom_stm.c @@ -16,19 +16,10 @@ */ #include -#include +#include /* Total bytes of available storage */ -ao_pos_t ao_storage_total = 4096; - -/* Block size - device is erased in these units. */ -ao_pos_t ao_storage_block = 1024; - -/* Byte offset of config block. Will be ao_storage_block bytes long */ -ao_pos_t ao_storage_config = 0; - -/* Storage unit size - device reads and writes must be within blocks of this size. */ -uint16_t ao_storage_unit = 1024; +const ao_pos_t ao_eeprom_total = 4096; /* Location of eeprom in address space */ #define stm_eeprom ((uint8_t *) 0x08080000) @@ -42,16 +33,6 @@ uint16_t ao_storage_unit = 1024; * the same contents, or append to an existing page easily enough */ -/* - * Erase the specified sector - */ -uint8_t -ao_storage_erase(ao_pos_t pos) __reentrant -{ - /* Not necessary */ - return 1; -} - static void ao_intflash_unlock(void) { @@ -131,16 +112,16 @@ ao_intflash_read(uint16_t pos) } /* - * Write to flash + * Write to eeprom */ uint8_t -ao_storage_device_write(ao_pos_t pos32, __xdata void *v, uint16_t len) __reentrant +ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len) { uint16_t pos = pos32; __xdata uint8_t *d = v; - if (pos >= ao_storage_total || pos + len > ao_storage_total) + if (pos >= ao_eeprom_total || pos + len > ao_eeprom_total) return 0; ao_intflash_unlock(); @@ -166,38 +147,26 @@ ao_storage_device_write(ao_pos_t pos32, __xdata void *v, uint16_t len) __reentra } /* - * Read from flash + * Read from eeprom */ uint8_t -ao_storage_device_read(ao_pos_t pos, __xdata void *v, uint16_t len) __reentrant +ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len) { uint8_t *d = v; - if (pos >= ao_storage_total || pos + len > ao_storage_total) + if (pos >= ao_eeprom_total || pos + len > ao_eeprom_total) return 0; while (len--) *d++ = ao_intflash_read(pos++); return 1; } -void -ao_storage_flush(void) __reentrant -{ -} - -void -ao_storage_setup(void) -{ -} - -void -ao_storage_device_info(void) __reentrant -{ - uint8_t i; - printf ("Using internal flash\n"); -} +/* + * Initialize eeprom + */ void -ao_storage_device_init(void) +ao_eeprom_init(void) { + /* Nothing to do here */ } diff --git a/src/telegps-v0.1/Makefile b/src/telegps-v0.1/Makefile index 170294e6..f5533d51 100644 --- a/src/telegps-v0.1/Makefile +++ b/src/telegps-v0.1/Makefile @@ -57,7 +57,6 @@ ALTOS_SRC = \ ao_fec_tx.c \ ao_rfpa0133.c \ ao_aprs.c \ - ao_storage.c \ ao_eeprom_stm.c \ ao_sdcard.c \ ao_bufio.c \ diff --git a/src/telegps-v0.1/ao_pins.h b/src/telegps-v0.1/ao_pins.h index 5bea2681..7ff59956 100644 --- a/src/telegps-v0.1/ao_pins.h +++ b/src/telegps-v0.1/ao_pins.h @@ -65,7 +65,10 @@ #define ao_gps_fifo (ao_stm_usart2.rx_fifo) #define HAS_EEPROM 1 -#define USE_INTERNAL_FLASH 1 +#define USE_INTERNAL_FLASH 0 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 + #define HAS_USB 1 #define HAS_BEEP 0 #define HAS_RADIO 1 diff --git a/src/telegps-v0.1/ao_telegps.c b/src/telegps-v0.1/ao_telegps.c index 68116bfb..bc37b504 100644 --- a/src/telegps-v0.1/ao_telegps.c +++ b/src/telegps-v0.1/ao_telegps.c @@ -18,6 +18,7 @@ #include #include #include +#include uint16_t ao_flight_number = 1; @@ -40,7 +41,7 @@ main(void) ao_dma_init(); ao_exti_init(); - ao_storage_init(); + ao_eeprom_init(); ao_serial_init(); diff --git a/src/telelco-v0.2/Makefile b/src/telelco-v0.2/Makefile index bc5f8571..2fb4db5e 100644 --- a/src/telelco-v0.2/Makefile +++ b/src/telelco-v0.2/Makefile @@ -48,7 +48,6 @@ ALTOS_SRC = \ ao_dma_stm.c \ ao_spi_stm.c \ ao_beep_stm.c \ - ao_storage.c \ ao_eeprom_stm.c \ ao_fast_timer.c \ ao_lcd_stm.c \ diff --git a/src/telelco-v0.2/ao_pins.h b/src/telelco-v0.2/ao_pins.h index d86782f3..62f221a1 100644 --- a/src/telelco-v0.2/ao_pins.h +++ b/src/telelco-v0.2/ao_pins.h @@ -43,6 +43,8 @@ #define HAS_EEPROM 1 #define USE_INTERNAL_FLASH 1 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 #define HAS_RADIO 1 diff --git a/src/telelco-v0.2/ao_telelco.c b/src/telelco-v0.2/ao_telelco.c index 66bf0ba1..d9f7c693 100644 --- a/src/telelco-v0.2/ao_telelco.c +++ b/src/telelco-v0.2/ao_telelco.c @@ -28,6 +28,7 @@ #include #include #include +#include int main(void) @@ -52,7 +53,7 @@ main(void) ao_quadrature_init(); ao_button_init(); - ao_storage_init(); + ao_eeprom_init(); ao_radio_init(); -- cgit v1.2.3 From 9c53ad6f8222878a26efecebd3bb1d1fe054a4b6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 12 Nov 2013 16:06:59 +0900 Subject: altos: Move TeleMetrum v2.0 to using internal eeprom for config This leaves the whole 8MB of flash for flight storage Signed-off-by: Keith Packard --- src/core/ao_config.c | 11 ++++++----- src/core/ao_config.h | 16 ++++++---------- src/core/ao_log.c | 11 ++++++----- src/telemetrum-v2.0/Makefile | 1 + src/telemetrum-v2.0/ao_pins.h | 3 +++ src/telemetrum-v2.0/ao_telemetrum.c | 2 ++ 6 files changed, 24 insertions(+), 20 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_config.c b/src/core/ao_config.c index 5567587b..a30ec64a 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -60,7 +60,8 @@ static void _ao_config_put(void) { ao_config_setup(); - ao_config_write(&ao_config, sizeof (ao_config)); + ao_config_erase(); + ao_config_write(0, &ao_config, sizeof (ao_config)); #if HAS_FLIGHT ao_log_write_erase(0); #endif @@ -97,7 +98,7 @@ _ao_config_get(void) * need before calling ao_storage_read here */ ao_config_setup(); - ao_config_read(&ao_config, sizeof (ao_config)); + ao_config_read(0, &ao_config, sizeof (ao_config)); #endif if (ao_config.major != AO_CONFIG_MAJOR) { ao_config.major = AO_CONFIG_MAJOR; @@ -466,7 +467,7 @@ void ao_config_log_set(void) __reentrant { uint16_t block = (uint16_t) (ao_storage_block >> 10); - uint16_t config = (uint16_t) (ao_storage_config >> 10); + uint16_t log_max = (uint16_t) (ao_storage_log_max >> 10); ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) @@ -475,8 +476,8 @@ ao_config_log_set(void) __reentrant 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 > config) - printf("Flight log max %d kB\n", config); + 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; diff --git a/src/core/ao_config.h b/src/core/ao_config.h index 5e38430e..e101af8e 100644 --- a/src/core/ao_config.h +++ b/src/core/ao_config.h @@ -31,14 +31,9 @@ #include #define ao_config_setup() ao_storage_setup() - -#define ao_config_write(bytes, len) do { \ - ao_storage_erase(ao_storage_config); \ - ao_storage_write(ao_storage_config, bytes, len); \ - } while (0) - -#define ao_config_read(bytes, len) ao_storage_read(ao_storage_config, bytes, len) - +#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 @@ -48,8 +43,9 @@ #include #define ao_config_setup() -#define ao_config_write(bytes, len) ao_eeprom_write(0, bytes, len) -#define ao_config_read(bytes, len) ao_eeprom_read(0, bytes, len) +#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 diff --git a/src/core/ao_log.c b/src/core/ao_log.c index 8bcb7707..1a1f5ff6 100644 --- a/src/core/ao_log.c +++ b/src/core/ao_log.c @@ -17,6 +17,7 @@ #include "ao.h" #include +#include __pdata uint32_t ao_log_current_pos; __pdata uint32_t ao_log_end_pos; @@ -48,7 +49,7 @@ static __xdata struct ao_log_erase erase; static uint32_t ao_log_erase_pos(uint8_t i) { - return i * sizeof (struct ao_log_erase) + AO_STORAGE_ERASE_LOG; + return i * sizeof (struct ao_log_erase); } void @@ -56,14 +57,14 @@ ao_log_write_erase(uint8_t pos) { erase.unused = 0x00; erase.flight = ao_flight_number; - ao_storage_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); - ao_storage_flush(); + ao_config_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); + ao_config_flush(); } static void ao_log_read_erase(uint8_t pos) { - ao_storage_read(ao_log_erase_pos(pos), &erase, sizeof (erase)); + ao_config_read(ao_log_erase_pos(pos), &erase, sizeof (erase)); } @@ -87,7 +88,7 @@ ao_log_erase_mark(void) static uint8_t ao_log_slots() { - return (uint8_t) (ao_storage_config / ao_config.flight_log_max); + return (uint8_t) (ao_storage_log_max / ao_config.flight_log_max); } uint32_t diff --git a/src/telemetrum-v2.0/Makefile b/src/telemetrum-v2.0/Makefile index be72d493..a5370224 100644 --- a/src/telemetrum-v2.0/Makefile +++ b/src/telemetrum-v2.0/Makefile @@ -70,6 +70,7 @@ ALTOS_SRC = \ ao_m25.c \ ao_usb_stm.c \ ao_exti_stm.c \ + ao_eeprom_stm.c \ ao_report.c \ ao_convert_pa.c \ ao_log.c \ diff --git a/src/telemetrum-v2.0/ao_pins.h b/src/telemetrum-v2.0/ao_pins.h index a7236b80..02f0f5e3 100644 --- a/src/telemetrum-v2.0/ao_pins.h +++ b/src/telemetrum-v2.0/ao_pins.h @@ -59,8 +59,11 @@ #define SERIAL_3_PC10_PC11 0 #define SERIAL_3_PD8_PD9 0 +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX (512 * 1024) #define HAS_EEPROM 1 #define USE_INTERNAL_FLASH 0 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 #define BEEPER_CHANNEL 4 diff --git a/src/telemetrum-v2.0/ao_telemetrum.c b/src/telemetrum-v2.0/ao_telemetrum.c index d79aba54..23556c6c 100644 --- a/src/telemetrum-v2.0/ao_telemetrum.c +++ b/src/telemetrum-v2.0/ao_telemetrum.c @@ -62,6 +62,8 @@ main(void) ao_mma655x_init(); #endif + ao_eeprom_init(); + ao_storage_init(); ao_flight_init(); -- cgit v1.2.3 From bf893a4149b05b97f18f9f487af805adef859d74 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 12 Nov 2013 16:22:49 +0900 Subject: altos: Make sure flight erase log comes after config blog Oops. When converting from ao_storage to ao_config, I accidentally had the flight erase log overwriting the config block. Signed-off-by: Keith Packard --- src/core/ao_log.c | 2 +- src/core/ao_storage.h | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_log.c b/src/core/ao_log.c index 1a1f5ff6..701c81ab 100644 --- a/src/core/ao_log.c +++ b/src/core/ao_log.c @@ -49,7 +49,7 @@ static __xdata struct ao_log_erase erase; static uint32_t ao_log_erase_pos(uint8_t i) { - return i * sizeof (struct ao_log_erase); + return i * sizeof (struct ao_log_erase) + AO_CONFIG_MAX_SIZE; } void diff --git a/src/core/ao_storage.h b/src/core/ao_storage.h index d6e95605..6cc6fcb7 100644 --- a/src/core/ao_storage.h +++ b/src/core/ao_storage.h @@ -51,8 +51,6 @@ extern __pdata ao_pos_t ao_storage_config; /* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */ extern __pdata uint16_t ao_storage_unit; -#define AO_STORAGE_ERASE_LOG (ao_storage_config + AO_CONFIG_MAX_SIZE) - /* Initialize above values. Can only be called once the OS is running */ void ao_storage_setup(void) __reentrant; -- cgit v1.2.3 From 2a9b0cdff5db03dc11b6ef69cf5436c834c3acc4 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Dec 2013 09:39:41 -0800 Subject: altos: Add lots more GPS data to mega log There's plenty of space in the GPS log packets to hold course, speed, climb and DOP values, so just stick them in. Signed-off-by: Keith Packard --- src/core/ao_gps_report_mega.c | 7 +++++++ src/core/ao_log.h | 10 ++++++++-- 2 files changed, 15 insertions(+), 2 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c index a66068ab..d13885dd 100644 --- a/src/core/ao_gps_report_mega.c +++ b/src/core/ao_gps_report_mega.c @@ -53,6 +53,13 @@ ao_gps_report_mega(void) 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) { diff --git a/src/core/ao_log.h b/src/core/ao_log.h index 4b09faeb..09f31188 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -253,8 +253,14 @@ struct ao_log_mega { uint8_t year; /* 18 */ uint8_t month; /* 19 */ uint8_t day; /* 20 */ - uint8_t pad; /* 21 */ - } gps; /* 22 */ + 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 */ -- cgit v1.2.3 From a1e4750a7d4af72e8e9086735885f48c9b56c18e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 8 Dec 2013 11:11:41 -0800 Subject: altos: Allow products to override default 100mA USB current This will allow products to specify their own current limit. Signed-off-by: Keith Packard --- src/core/ao_product.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_product.c b/src/core/ao_product.c index ec91b978..b9327bac 100644 --- a/src/core/ao_product.c +++ b/src/core/ao_product.c @@ -27,6 +27,12 @@ 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 [] = @@ -55,7 +61,7 @@ AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = 0x01, /* bConfigurationValue */ 0x00, /* iConfiguration */ 0xC0, /* bmAttributes */ - 0x32, /* bMaxPower */ + AO_USB_MAX_POWER >> 1, /* bMaxPower, 2mA units */ /* Control class interface */ 0x09, -- cgit v1.2.3 From 2ecb6a8276b2ce40d2a4da586dbc17581cfda26d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 17 Dec 2013 20:23:00 -0800 Subject: altos: Broke TeleMetrum GPS reporting by holding the GPS mutex too much We can't hold the GPS mutex while waiting for the GPS receiver to load data as it protects the GPS data with the GPS mutex. Signed-off-by: Keith Packard --- src/core/ao_gps_report.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/core') diff --git a/src/core/ao_gps_report.c b/src/core/ao_gps_report.c index 8d15c083..07201ac2 100644 --- a/src/core/ao_gps_report.c +++ b/src/core/ao_gps_report.c @@ -27,9 +27,9 @@ ao_gps_report(void) uint8_t new; for (;;) { - ao_mutex_get(&ao_gps_mutex); 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) -- cgit v1.2.3 From ee4279613b4757453d0d8f8afc06037c61eeb520 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 18 Dec 2013 20:32:05 -0800 Subject: altos: Try IMU self-test 10 times before giving up This should keep the device from failing to boot unless the IMU is actually broken. Oh, and if self test does fail, this places the flight computer in 'Invalid' state rather than panic. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 8 ++++ src/core/ao_flight.h | 4 ++ src/drivers/ao_mpu6000.c | 104 ++++++++++++++++++++++++-------------------- src/telemega-v0.1/ao_pins.h | 1 + src/telemega-v1.0/ao_pins.h | 3 +- 5 files changed, 70 insertions(+), 50 deletions(-) (limited to 'src/core') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 4a53bdc6..463ff4a2 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -46,6 +46,11 @@ __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_IMU +/* 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 @@ -99,6 +104,9 @@ ao_flight(void) 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 || +#if HAS_IMU + ao_sensor_errors || +#endif ao_ground_height < -1000 || ao_ground_height > 7000) { diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h index ac3e9cfb..c7c02ccf 100644 --- a/src/core/ao_flight.h +++ b/src/core/ao_flight.h @@ -41,6 +41,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 +extern __xdata uint8_t ao_sensor_errors; +#endif + extern __pdata uint16_t ao_launch_time; extern __pdata uint8_t ao_flight_force_idle; diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index 5e75b78a..f8ce7346 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -137,10 +137,10 @@ ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which) { int16_t diff = test - normal; - if (diff < MPU6000_ST_ACCEL(16) / 2) { + if (diff < MPU6000_ST_ACCEL(16) / 4) { return 1; } - if (diff > MPU6000_ST_ACCEL(16) * 2) { + if (diff > MPU6000_ST_ACCEL(16) * 4) { return 1; } return 0; @@ -153,10 +153,10 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which) if (diff < 0) diff = -diff; - if (diff < MPU6000_ST_GYRO(2000) / 2) { + if (diff < MPU6000_ST_GYRO(2000) / 4) { return 1; } - if (diff > MPU6000_ST_GYRO(2000) * 2) { + if (diff > MPU6000_ST_GYRO(2000) * 4) { return 1; } return 0; @@ -177,11 +177,14 @@ _ao_mpu6000_wait_alive(void) ao_panic(AO_PANIC_SELF_TEST_MPU6000); } +#define ST_TRIES 10 + static void _ao_mpu6000_setup(void) { struct ao_mpu6000_sample normal_mode, test_mode; - int errors =0; + int errors; + int st_tries; if (ao_mpu6000_configured) return; @@ -236,23 +239,6 @@ _ao_mpu6000_setup(void) (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG)); - /* Configure accelerometer to +/-16G in self-test mode */ - _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, - (1 << MPU600_ACCEL_CONFIG_XA_ST) | - (1 << MPU600_ACCEL_CONFIG_YA_ST) | - (1 << MPU600_ACCEL_CONFIG_ZA_ST) | - (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); - - /* Configure gyro to +/- 2000°/s in self-test mode */ - _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, - (1 << MPU600_GYRO_CONFIG_XG_ST) | - (1 << MPU600_GYRO_CONFIG_YG_ST) | - (1 << MPU600_GYRO_CONFIG_ZG_ST) | - (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); - - ao_delay(AO_MS_TO_TICKS(200)); - _ao_mpu6000_sample(&test_mode); - #if TRIDGE // read the product ID rev c has 1/2 the sensitivity of rev d _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); @@ -268,36 +254,58 @@ _ao_mpu6000_setup(void) register_write(MPUREG_ACCEL_CONFIG,2<<3); } hal.scheduler->delay(1); - #endif - /* Configure accelerometer to +/-16G */ - _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, - (0 << MPU600_ACCEL_CONFIG_XA_ST) | - (0 << MPU600_ACCEL_CONFIG_YA_ST) | - (0 << MPU600_ACCEL_CONFIG_ZA_ST) | - (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); - - /* Configure gyro to +/- 2000°/s */ - _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, - (0 << MPU600_GYRO_CONFIG_XG_ST) | - (0 << MPU600_GYRO_CONFIG_YG_ST) | - (0 << MPU600_GYRO_CONFIG_ZG_ST) | - (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); - - ao_delay(AO_MS_TO_TICKS(10)); - _ao_mpu6000_sample(&normal_mode); + for (st_tries = 0; st_tries < ST_TRIES; st_tries++) { + errors = 0; + + /* Configure accelerometer to +/-16G in self-test mode */ + _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, + (1 << MPU600_ACCEL_CONFIG_XA_ST) | + (1 << MPU600_ACCEL_CONFIG_YA_ST) | + (1 << MPU600_ACCEL_CONFIG_ZA_ST) | + (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); + + /* Configure gyro to +/- 2000°/s in self-test mode */ + _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, + (1 << MPU600_GYRO_CONFIG_XG_ST) | + (1 << MPU600_GYRO_CONFIG_YG_ST) | + (1 << MPU600_GYRO_CONFIG_ZG_ST) | + (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); + + ao_delay(AO_MS_TO_TICKS(200)); + _ao_mpu6000_sample(&test_mode); + + /* Configure accelerometer to +/-16G */ + _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, + (0 << MPU600_ACCEL_CONFIG_XA_ST) | + (0 << MPU600_ACCEL_CONFIG_YA_ST) | + (0 << MPU600_ACCEL_CONFIG_ZA_ST) | + (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); + + /* Configure gyro to +/- 2000°/s */ + _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, + (0 << MPU600_GYRO_CONFIG_XG_ST) | + (0 << MPU600_GYRO_CONFIG_YG_ST) | + (0 << MPU600_GYRO_CONFIG_ZG_ST) | + (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); + + ao_delay(AO_MS_TO_TICKS(200)); + _ao_mpu6000_sample(&normal_mode); - errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x"); - errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y"); - errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z"); - - errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x"); - errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y"); - errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z"); + errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x"); + errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y"); + errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z"); + + errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x"); + errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y"); + errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z"); + if (!errors) + break; + } - if (errors) - ao_panic(AO_PANIC_SELF_TEST_MPU6000); + if (st_tries == ST_TRIES) + ao_sensor_errors = 1; /* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */ _ao_mpu6000_reg_write(MPU6000_CONFIG, diff --git a/src/telemega-v0.1/ao_pins.h b/src/telemega-v0.1/ao_pins.h index 7ba3a1a7..daeb9f17 100644 --- a/src/telemega-v0.1/ao_pins.h +++ b/src/telemega-v0.1/ao_pins.h @@ -316,6 +316,7 @@ struct ao_adc { #define AO_MPU6000_INT_PORT (&stm_gpioc) #define AO_MPU6000_INT_PIN 13 #define AO_MPU6000_I2C_INDEX STM_I2C_INDEX(1) +#define HAS_IMU 1 /* * mma655x diff --git a/src/telemega-v1.0/ao_pins.h b/src/telemega-v1.0/ao_pins.h index 7ff676ea..95644dae 100644 --- a/src/telemega-v1.0/ao_pins.h +++ b/src/telemega-v1.0/ao_pins.h @@ -318,8 +318,7 @@ struct ao_adc { #define AO_MPU6000_SPI_BUS AO_SPI_1_PE13_PE14_PE15 #define AO_MPU6000_SPI_CS_PORT (&stm_gpiod) #define AO_MPU6000_SPI_CS_PIN 2 - -#define HAS_HIGHG_ACCEL 1 +#define HAS_IMU 1 /* * mma655x -- cgit v1.2.3