ao_cmd_hex(void);
void
-ao_cmd_decimal(void);
+ao_cmd_decimal(void) __reentrant;
/* Read a single hex nibble off stdin. */
uint8_t
}
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();
#include <ao_int64.h>
-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;
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;
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)
}
}
-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)
}
}
-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);
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)
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))
__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;
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;
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;
#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];
#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);
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);
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_ */
*/
#include <ao_ms5607.h>
+#include <ao_int64.h>
+
+#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
}
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 \
ao_storage.c \
ao_task.c \
ao_telemetry.c \
- ao_freq.c
+ ao_freq.c \
+ ao_int64.c
CC1111_SRC = \
ao_adc.c \
#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)
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
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
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
+#define __data
+#define __pdata
+#define __xdata
+#define __reentrant
+
#include <ao_int64.h>
#include <ao_int64.c>
#include <stdio.h>
--- /dev/null
+/*
+ * Copyright © 2013 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.
+ */
+
+#define __xdata
+#define __data
+#define __pdata
+#define __reentrant
+
+#include <stdint.h>
+#include <ao_ms5607.h>
+
+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 <ao_ms5607_convert.c>
+#define ao_ms5607_convert ao_ms5607_convert_8051
+#include <ao_ms5607_convert_8051.c>
+#include <ao_int64.c>
+#include <stdio.h>
+#include <stdlib.h>
+
+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 <stdlib.h>
+
+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;
+}