diff options
Diffstat (limited to 'src/core')
| -rw-r--r-- | src/core/ao.h | 24 | ||||
| -rw-r--r-- | src/core/ao_monitor.c | 10 | ||||
| -rw-r--r-- | src/core/ao_sqrt.c | 46 |
3 files changed, 76 insertions, 4 deletions
diff --git a/src/core/ao.h b/src/core/ao.h index 558d0e38..9b8d3270 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -996,7 +996,11 @@ ao_spi_slave(void); */ #define AO_MAX_CALLSIGN 8 #define AO_MAX_VERSION 8 +#if LEGACY_MONITOR #define AO_MAX_TELEMETRY 128 +#else +#define AO_MAX_TELEMETRY 32 +#endif struct ao_telemetry_generic { uint16_t serial; /* 0 */ @@ -1156,6 +1160,12 @@ union ao_telemetry_all { struct ao_telemetry_baro baro; }; +struct ao_telemetry_all_recv { + union ao_telemetry_all telemetry; + int8_t rssi; + uint8_t status; +}; + /* * ao_gps.c */ @@ -1372,9 +1382,10 @@ extern const char const * const ao_state_names[]; #define AO_MONITOR_RING 8 union ao_monitor { - struct ao_telemetry_raw_recv raw; - struct ao_telemetry_orig_recv orig; - struct ao_telemetry_tiny_recv tiny; + struct ao_telemetry_raw_recv raw; + struct ao_telemetry_all_recv all; + struct ao_telemetry_orig_recv orig; + struct ao_telemetry_tiny_recv tiny; }; extern __xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; @@ -1873,4 +1884,11 @@ void ao_battery_init(void); #endif /* BATTERY_PIN */ +/* + * ao_sqrt.c + */ + +uint32_t +ao_sqrt(uint32_t op); + #endif /* _AO_H_ */ diff --git a/src/core/ao_monitor.c b/src/core/ao_monitor.c index 5a6f61dd..f7795fe4 100644 --- a/src/core/ao_monitor.c +++ b/src/core/ao_monitor.c @@ -26,6 +26,10 @@ #error Must define LEGACY_MONITOR #endif +#ifndef HAS_MONITOR_PUT +#define HAS_MONIOTOR_PUT 1 +#endif + __data uint8_t ao_monitoring; __pdata uint8_t ao_monitor_led; @@ -73,6 +77,7 @@ ao_monitor_blink(void) } } +#if HAS_MONITOR_PUT void ao_monitor_put(void) { @@ -260,9 +265,10 @@ ao_monitor_put(void) ao_usb_flush(); } } +__xdata struct ao_task ao_monitor_put_task; +#endif __xdata struct ao_task ao_monitor_get_task; -__xdata struct ao_task ao_monitor_put_task; __xdata struct ao_task ao_monitor_blink_task; void @@ -293,7 +299,9 @@ ao_monitor_init(uint8_t monitor_led, uint8_t monitoring) __reentrant ao_monitoring = monitoring; ao_cmd_register(&ao_monitor_cmds[0]); ao_add_task(&ao_monitor_get_task, ao_monitor_get, "monitor_get"); +#if HAS_MONITOR_PUT ao_add_task(&ao_monitor_put_task, ao_monitor_put, "monitor_put"); +#endif if (ao_monitor_led) ao_add_task(&ao_monitor_blink_task, ao_monitor_blink, "monitor_blink"); } diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c new file mode 100644 index 00000000..09c2e319 --- /dev/null +++ b/src/core/ao_sqrt.c @@ -0,0 +1,46 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +/* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */ +/** + * int_sqrt - rough approximation to sqrt + * @x: integer of which to calculate the sqrt + * + * A very rough approximation to the sqrt() function. + */ + +uint32_t +ao_sqrt(uint32_t op) +{ + uint32_t res = 0; + uint32_t one = 1UL << (sizeof (one) * 8 - 2); + + while (one > op) + one >>= 2; + + while (one != 0) { + if (op >= res + one) { + op = op - (res + one); + res = res + 2 * one; + } + res /= 2; + one /= 4; + } + return res; +} |