summaryrefslogtreecommitdiff
path: root/src/kernel
diff options
context:
space:
mode:
Diffstat (limited to 'src/kernel')
-rw-r--r--src/kernel/ao.h14
-rw-r--r--src/kernel/ao_cmd.c3
-rw-r--r--src/kernel/ao_config.c3
-rw-r--r--src/kernel/ao_convert.c10
-rw-r--r--src/kernel/ao_convert_pa.c16
-rw-r--r--src/kernel/ao_convert_pa_test.c1
-rw-r--r--src/kernel/ao_data.h6
-rw-r--r--src/kernel/ao_flight.c12
-rw-r--r--src/kernel/ao_gps_print.c6
-rw-r--r--src/kernel/ao_gps_report.c8
-rw-r--r--src/kernel/ao_gps_report_mega.c7
-rw-r--r--src/kernel/ao_gps_report_metrum.c4
-rw-r--r--src/kernel/ao_gps_show.c8
-rw-r--r--src/kernel/ao_kalman.c147
-rw-r--r--src/kernel/ao_log.h25
-rw-r--r--src/kernel/ao_log_gps.c3
-rw-r--r--src/kernel/ao_log_gps.h3
-rw-r--r--src/kernel/ao_microkalman.c12
-rw-r--r--src/kernel/ao_sample.c2
-rw-r--r--src/kernel/ao_sample.h56
-rw-r--r--src/kernel/ao_telemetry.c2
-rw-r--r--src/kernel/ao_telemetry.h34
-rw-r--r--src/kernel/ao_tracker.c13
23 files changed, 236 insertions, 159 deletions
diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index a225bc4a..ad5bbf8e 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -278,15 +278,17 @@ ao_report_init(void);
* Given raw data, convert to SI units
*/
+#if HAS_BARO
/* pressure from the sensor to altitude in meters */
-int16_t
-ao_pres_to_altitude(int16_t pres) __reentrant;
+alt_t
+ao_pres_to_altitude(pres_t pres) __reentrant;
-int16_t
-ao_altitude_to_pres(int16_t alt) __reentrant;
+pres_t
+ao_altitude_to_pres(alt_t alt) __reentrant;
int16_t
ao_temp_to_dC(int16_t temp) __reentrant;
+#endif
/*
* ao_convert_pa.c
@@ -296,11 +298,13 @@ ao_temp_to_dC(int16_t temp) __reentrant;
#include <ao_data.h>
+#if HAS_BARO
alt_t
-ao_pa_to_altitude(int32_t pa);
+ao_pa_to_altitude(pres_t pa);
int32_t
ao_altitude_to_pa(alt_t alt);
+#endif
#if HAS_DBG
#include <ao_dbg.h>
diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c
index 0052bdce..d2f583ef 100644
--- a/src/kernel/ao_cmd.c
+++ b/src/kernel/ao_cmd.c
@@ -290,6 +290,9 @@ version(void)
#if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND)
"program-space %u\n"
#endif
+#if AO_VALUE_32
+ "altitude-32 1\n"
+#endif
, ao_manufacturer
, ao_product
, ao_serial_number
diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
index 32a0967c..d73a3733 100644
--- a/src/kernel/ao_config.c
+++ b/src/kernel/ao_config.c
@@ -518,6 +518,9 @@ ao_config_radio_rate_set(void) __reentrant
ao_telemetry_reset_interval();
#endif
_ao_config_edit_finish();
+#if HAS_RADIO_RECV
+ ao_radio_recv_abort();
+#endif
}
#endif
diff --git a/src/kernel/ao_convert.c b/src/kernel/ao_convert.c
index aa9b5f48..db1f2301 100644
--- a/src/kernel/ao_convert.c
+++ b/src/kernel/ao_convert.c
@@ -19,14 +19,16 @@
#include "ao.h"
#endif
-static const int16_t altitude_table[] = {
+#include <ao_sample.h>
+
+static const ao_v_t altitude_table[] = {
#include "altitude.h"
};
#define ALT_FRAC_SCALE (1 << ALT_FRAC_BITS)
#define ALT_FRAC_MASK (ALT_FRAC_SCALE - 1)
-int16_t
+ao_v_t
ao_pres_to_altitude(int16_t pres) __reentrant
{
uint8_t o;
@@ -43,9 +45,9 @@ ao_pres_to_altitude(int16_t pres) __reentrant
#if AO_NEED_ALTITUDE_TO_PRES
int16_t
-ao_altitude_to_pres(int16_t alt) __reentrant
+ao_altitude_to_pres(ao_v_t alt) __reentrant
{
- int16_t span, sub_span;
+ ao_v_t span, sub_span;
uint8_t l, h, m;
int32_t pres;
diff --git a/src/kernel/ao_convert_pa.c b/src/kernel/ao_convert_pa.c
index 20162c1f..410815b6 100644
--- a/src/kernel/ao_convert_pa.c
+++ b/src/kernel/ao_convert_pa.c
@@ -39,11 +39,11 @@ static const alt_t altitude_table[] AO_CONST_ATTRIB = {
#define ALT_MASK (ALT_SCALE - 1)
alt_t
-ao_pa_to_altitude(int32_t pa)
+ao_pa_to_altitude(pres_t pa)
{
int16_t o;
int16_t part;
- int32_t low, high;
+ alt_t low, high;
if (pa < 0)
pa = 0;
@@ -52,16 +52,16 @@ ao_pa_to_altitude(int32_t pa)
o = pa >> ALT_SHIFT;
part = pa & ALT_MASK;
- low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part);
- high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1);
+ low = (alt_t) FETCH_ALT(o) * (ALT_SCALE - part);
+ high = (alt_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1);
return (low + high) >> ALT_SHIFT;
}
#ifdef AO_CONVERT_TEST
-int32_t
-ao_altitude_to_pa(int32_t alt)
+pres_t
+ao_altitude_to_pa(alt_t alt)
{
- int32_t span, sub_span;
+ alt_t span, sub_span;
uint16_t l, h, m;
int32_t pa;
@@ -76,7 +76,7 @@ ao_altitude_to_pa(int32_t alt)
}
span = altitude_table[l] - altitude_table[h];
sub_span = altitude_table[l] - alt;
- pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span;
+ pa = ((((alt_t) l * (span - sub_span) + (alt_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span;
if (pa > 120000)
pa = 120000;
if (pa < 0)
diff --git a/src/kernel/ao_convert_pa_test.c b/src/kernel/ao_convert_pa_test.c
index 7d5b1922..95422862 100644
--- a/src/kernel/ao_convert_pa_test.c
+++ b/src/kernel/ao_convert_pa_test.c
@@ -18,6 +18,7 @@
#include <stdint.h>
#define AO_CONVERT_TEST
typedef int32_t alt_t;
+typedef int32_t pres_t;
#include "ao_host.h"
#include "ao_convert_pa.c"
diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index c4b062fd..8f75ad87 100644
--- a/src/kernel/ao_data.h
+++ b/src/kernel/ao_data.h
@@ -117,9 +117,7 @@ extern volatile __data uint8_t ao_data_count;
typedef int32_t pres_t;
-#ifndef AO_ALT_TYPE
#define AO_ALT_TYPE int32_t
-#endif
typedef AO_ALT_TYPE alt_t;
@@ -146,10 +144,6 @@ typedef int16_t alt_t;
#endif
-#if !HAS_BARO
-typedef int16_t alt_t;
-#endif
-
/*
* Need a few macros to pull data from the sensors:
*
diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 2b433ee9..251dbc02 100644
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -60,10 +60,10 @@ __xdata uint8_t ao_sensor_errors;
* resting
*/
static __data uint16_t ao_interval_end;
-static __data int16_t ao_interval_min_height;
-static __data int16_t ao_interval_max_height;
+static __data ao_v_t ao_interval_min_height;
+static __data ao_v_t ao_interval_max_height;
#if HAS_ACCEL
-static __data int16_t ao_coast_avg_accel;
+static __data ao_v_t ao_coast_avg_accel;
#endif
__pdata uint8_t ao_flight_force_idle;
@@ -398,14 +398,14 @@ ao_flight(void)
}
#if HAS_FLIGHT_DEBUG
-static inline int int_part(int16_t i) { return i >> 4; }
-static inline int frac_part(int16_t i) { return ((i & 0xf) * 100 + 8) / 16; }
+static inline int int_part(ao_v_t i) { return i >> 4; }
+static inline int frac_part(ao_v_t i) { return ((i & 0xf) * 100 + 8) / 16; }
static void
ao_flight_dump(void)
{
#if HAS_ACCEL
- int16_t accel;
+ ao_v_t accel;
accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
#endif
diff --git a/src/kernel/ao_gps_print.c b/src/kernel/ao_gps_print.c
index 47c945d7..d26021da 100644
--- a/src/kernel/ao_gps_print.c
+++ b/src/kernel/ao_gps_print.c
@@ -20,6 +20,10 @@
#endif
#include "ao_telem.h"
+#ifndef AO_TELEMETRY_LOCATION_ALTITUDE
+#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((l)->altitude)
+#endif
+
void
ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant
{
@@ -42,7 +46,7 @@ ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant
AO_TELEM_GPS_ALTITUDE " %d ",
(long) gps_data->latitude,
(long) gps_data->longitude,
- gps_data->altitude);
+ AO_TELEMETRY_LOCATION_ALTITUDE(gps_data));
if (gps_data->flags & AO_GPS_DATE_VALID)
printf(AO_TELEM_GPS_YEAR " %d "
diff --git a/src/kernel/ao_gps_report.c b/src/kernel/ao_gps_report.c
index 07201ac2..7ef98a97 100644
--- a/src/kernel/ao_gps_report.c
+++ b/src/kernel/ao_gps_report.c
@@ -52,8 +52,12 @@ ao_gps_report(void)
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;
+ gps_log.u.gps_altitude.altitude_low = gps_data.altitude_low;
+#if HAS_WIDE_GPS
+ gps_log.u.gps_altitude.altitude_high = gps_data.altitude_high;
+#else
+ gps_log.u.gps_altitude.altitude_high = 0xffff;
+#endif
ao_log_data(&gps_log);
if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) {
gps_log.type = AO_LOG_GPS_DATE;
diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c
index cb0c0fd9..f3711fb1 100644
--- a/src/kernel/ao_gps_report_mega.c
+++ b/src/kernel/ao_gps_report_mega.c
@@ -78,7 +78,8 @@ ao_gps_report_mega(void)
#if GPS_SPARSE_LOG
/* Don't log data if GPS has a fix and hasn't moved for a while */
if ((gps_data.flags & AO_GPS_VALID) &&
- !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude, gps_data.altitude))
+ !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude,
+ AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data))
continue;
#endif
if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
@@ -87,8 +88,8 @@ ao_gps_report_mega(void)
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.altitude_low = gps_data.altitude_low;
+ gps_log.u.gps.altitude_high = gps_data.altitude_high;
gps_log.u.gps.hour = gps_data.hour;
gps_log.u.gps.minute = gps_data.minute;
gps_log.u.gps.second = gps_data.second;
diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c
index 696a833b..8ce074fe 100644
--- a/src/kernel/ao_gps_report_metrum.c
+++ b/src/kernel/ao_gps_report_metrum.c
@@ -44,7 +44,8 @@ ao_gps_report_metrum(void)
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;
+ gps_log.u.gps.altitude_low = gps_data.altitude_low;
+ gps_log.u.gps.altitude_high = gps_data.altitude_high;
ao_log_metrum(&gps_log);
gps_log.type = AO_LOG_GPS_TIME;
@@ -55,6 +56,7 @@ ao_gps_report_metrum(void)
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;
+ gps_log.u.gps_time.pdop = gps_data.pdop;
ao_log_metrum(&gps_log);
}
diff --git a/src/kernel/ao_gps_show.c b/src/kernel/ao_gps_show.c
index 3a05e35a..e45cd795 100644
--- a/src/kernel/ao_gps_show.c
+++ b/src/kernel/ao_gps_show.c
@@ -19,6 +19,8 @@
#include <ao.h>
#endif
+#include <ao_data.h>
+
void
ao_gps_show(void) __reentrant
{
@@ -27,7 +29,11 @@ ao_gps_show(void) __reentrant
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);
+#if HAS_WIDE_GPS
+ printf ("Alt: %ld\n", (long) AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data));
+#else
+ printf ("Alt: %d\n", AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data));
+#endif
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++)
diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c
index 9aea1f14..7b0f8145 100644
--- a/src/kernel/ao_kalman.c
+++ b/src/kernel/ao_kalman.c
@@ -23,32 +23,31 @@
#include "ao_sample.h"
#include "ao_kalman.h"
+static __pdata ao_k_t ao_k_height;
+static __pdata ao_k_t ao_k_speed;
+static __pdata ao_k_t ao_k_accel;
-static __pdata int32_t ao_k_height;
-static __pdata int32_t ao_k_speed;
-static __pdata int32_t ao_k_accel;
+#define AO_K_STEP_100 to_fix_v(0.01)
+#define AO_K_STEP_2_2_100 to_fix_v(0.00005)
-#define AO_K_STEP_100 to_fix16(0.01)
-#define AO_K_STEP_2_2_100 to_fix16(0.00005)
+#define AO_K_STEP_10 to_fix_v(0.1)
+#define AO_K_STEP_2_2_10 to_fix_v(0.005)
-#define AO_K_STEP_10 to_fix16(0.1)
-#define AO_K_STEP_2_2_10 to_fix16(0.005)
+#define AO_K_STEP_1 to_fix_v(1)
+#define AO_K_STEP_2_2_1 to_fix_v(0.5)
-#define AO_K_STEP_1 to_fix16(1)
-#define AO_K_STEP_2_2_1 to_fix16(0.5)
+__pdata ao_v_t ao_height;
+__pdata ao_v_t ao_speed;
+__pdata ao_v_t ao_accel;
+__xdata ao_v_t ao_max_height;
+static __pdata ao_k_t ao_avg_height_scaled;
+__xdata ao_v_t ao_avg_height;
-__pdata int16_t ao_height;
-__pdata int16_t ao_speed;
-__pdata int16_t ao_accel;
-__xdata int16_t ao_max_height;
-static __pdata int32_t ao_avg_height_scaled;
-__xdata int16_t ao_avg_height;
-
-__pdata int16_t ao_error_h;
-__pdata int16_t ao_error_h_sq_avg;
+__pdata ao_v_t ao_error_h;
+__pdata ao_v_t ao_error_h_sq_avg;
#if HAS_ACCEL
-__pdata int16_t ao_error_a;
+__pdata ao_v_t ao_error_a;
#endif
static void
@@ -56,40 +55,40 @@ ao_kalman_predict(void)
{
#ifdef AO_FLIGHT_TEST
if (ao_sample_tick - ao_sample_prev_tick > 50) {
- ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 +
- (int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
- ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1;
+ ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_1 +
+ (ao_k_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
+ ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_1;
return;
}
if (ao_sample_tick - ao_sample_prev_tick > 5) {
- ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 +
- (int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
- ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10;
+ ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_10 +
+ (ao_k_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
+ ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_10;
return;
}
if (ao_flight_debug) {
printf ("predict speed %g + (%g * %g) = %g\n",
ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0,
- (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
+ (ao_k_speed + (ao_k_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
}
#endif
- ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 +
- (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
- ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100;
+ ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_100 +
+ (ao_k_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
+ ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100;
}
static void
ao_kalman_err_height(void)
{
- int16_t e;
- int16_t height_distrust;
+ ao_v_t e;
+ ao_v_t height_distrust;
#if HAS_ACCEL
- int16_t speed_distrust;
+ ao_v_t speed_distrust;
#endif
- ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16);
+ ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16);
e = ao_error_h;
if (e < 0)
@@ -123,7 +122,7 @@ ao_kalman_err_height(void)
#endif
if (height_distrust > 0x100)
height_distrust = 0x100;
- ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8);
+ ao_error_h = (ao_v_t) (((ao_k_t) ao_error_h * (0x100 - height_distrust)) >> 8);
#ifdef AO_FLIGHT_TEST
if (ao_flight_debug) {
printf("over height %g over speed %g distrust: %g height: error %d -> %d\n",
@@ -142,21 +141,21 @@ ao_kalman_correct_baro(void)
ao_kalman_err_height();
#ifdef AO_FLIGHT_TEST
if (ao_sample_tick - ao_sample_prev_tick > 50) {
- ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h;
- ao_k_speed += (int32_t) AO_BARO_K1_1 * ao_error_h;
- ao_k_accel += (int32_t) AO_BARO_K2_1 * ao_error_h;
+ ao_k_height += (ao_k_t) AO_BARO_K0_1 * ao_error_h;
+ ao_k_speed += (ao_k_t) AO_BARO_K1_1 * ao_error_h;
+ ao_k_accel += (ao_k_t) AO_BARO_K2_1 * ao_error_h;
return;
}
if (ao_sample_tick - ao_sample_prev_tick > 5) {
- ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h;
- ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h;
- ao_k_accel += (int32_t) AO_BARO_K2_10 * ao_error_h;
+ ao_k_height += (ao_k_t) AO_BARO_K0_10 * ao_error_h;
+ ao_k_speed += (ao_k_t) AO_BARO_K1_10 * ao_error_h;
+ ao_k_accel += (ao_k_t) AO_BARO_K2_10 * ao_error_h;
return;
}
#endif
- ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h;
- ao_k_speed += (int32_t) AO_BARO_K1_100 * ao_error_h;
- ao_k_accel += (int32_t) AO_BARO_K2_100 * ao_error_h;
+ ao_k_height += (ao_k_t) AO_BARO_K0_100 * ao_error_h;
+ ao_k_speed += (ao_k_t) AO_BARO_K1_100 * ao_error_h;
+ ao_k_accel += (ao_k_t) AO_BARO_K2_100 * ao_error_h;
}
#if HAS_ACCEL
@@ -164,7 +163,7 @@ ao_kalman_correct_baro(void)
static void
ao_kalman_err_accel(void)
{
- int32_t accel;
+ ao_k_t accel;
accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
@@ -187,18 +186,18 @@ ao_kalman_correct_both(void)
(double) ao_error_h, AO_BOTH_K10_1 / 65536.0,
(double) ao_error_a, AO_BOTH_K11_1 / 65536.0,
(ao_k_speed +
- (int32_t) AO_BOTH_K10_1 * ao_error_h +
- (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
+ (ao_k_t) AO_BOTH_K10_1 * ao_error_h +
+ (ao_k_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
}
ao_k_height +=
- (int32_t) AO_BOTH_K00_1 * ao_error_h +
- (int32_t) AO_BOTH_K01_1 * ao_error_a;
+ (ao_k_t) AO_BOTH_K00_1 * ao_error_h +
+ (ao_k_t) AO_BOTH_K01_1 * ao_error_a;
ao_k_speed +=
- (int32_t) AO_BOTH_K10_1 * ao_error_h +
- (int32_t) AO_BOTH_K11_1 * ao_error_a;
+ (ao_k_t) AO_BOTH_K10_1 * ao_error_h +
+ (ao_k_t) AO_BOTH_K11_1 * ao_error_a;
ao_k_accel +=
- (int32_t) AO_BOTH_K20_1 * ao_error_h +
- (int32_t) AO_BOTH_K21_1 * ao_error_a;
+ (ao_k_t) AO_BOTH_K20_1 * ao_error_h +
+ (ao_k_t) AO_BOTH_K21_1 * ao_error_a;
return;
}
if (ao_sample_tick - ao_sample_prev_tick > 5) {
@@ -208,18 +207,18 @@ ao_kalman_correct_both(void)
(double) ao_error_h, AO_BOTH_K10_10 / 65536.0,
(double) ao_error_a, AO_BOTH_K11_10 / 65536.0,
(ao_k_speed +
- (int32_t) AO_BOTH_K10_10 * ao_error_h +
- (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
+ (ao_k_t) AO_BOTH_K10_10 * ao_error_h +
+ (ao_k_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
}
ao_k_height +=
- (int32_t) AO_BOTH_K00_10 * ao_error_h +
- (int32_t) AO_BOTH_K01_10 * ao_error_a;
+ (ao_k_t) AO_BOTH_K00_10 * ao_error_h +
+ (ao_k_t) AO_BOTH_K01_10 * ao_error_a;
ao_k_speed +=
- (int32_t) AO_BOTH_K10_10 * ao_error_h +
- (int32_t) AO_BOTH_K11_10 * ao_error_a;
+ (ao_k_t) AO_BOTH_K10_10 * ao_error_h +
+ (ao_k_t) AO_BOTH_K11_10 * ao_error_a;
ao_k_accel +=
- (int32_t) AO_BOTH_K20_10 * ao_error_h +
- (int32_t) AO_BOTH_K21_10 * ao_error_a;
+ (ao_k_t) AO_BOTH_K20_10 * ao_error_h +
+ (ao_k_t) AO_BOTH_K21_10 * ao_error_a;
return;
}
if (ao_flight_debug) {
@@ -228,19 +227,19 @@ ao_kalman_correct_both(void)
(double) ao_error_h, AO_BOTH_K10_100 / 65536.0,
(double) ao_error_a, AO_BOTH_K11_100 / 65536.0,
(ao_k_speed +
- (int32_t) AO_BOTH_K10_100 * ao_error_h +
- (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
+ (ao_k_t) AO_BOTH_K10_100 * ao_error_h +
+ (ao_k_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
}
#endif
ao_k_height +=
- (int32_t) AO_BOTH_K00_100 * ao_error_h +
- (int32_t) AO_BOTH_K01_100 * ao_error_a;
+ (ao_k_t) AO_BOTH_K00_100 * ao_error_h +
+ (ao_k_t) AO_BOTH_K01_100 * ao_error_a;
ao_k_speed +=
- (int32_t) AO_BOTH_K10_100 * ao_error_h +
- (int32_t) AO_BOTH_K11_100 * ao_error_a;
+ (ao_k_t) AO_BOTH_K10_100 * ao_error_h +
+ (ao_k_t) AO_BOTH_K11_100 * ao_error_a;
ao_k_accel +=
- (int32_t) AO_BOTH_K20_100 * ao_error_h +
- (int32_t) AO_BOTH_K21_100 * ao_error_a;
+ (ao_k_t) AO_BOTH_K20_100 * ao_error_h +
+ (ao_k_t) AO_BOTH_K21_100 * ao_error_a;
}
#else
@@ -251,14 +250,14 @@ ao_kalman_correct_accel(void)
ao_kalman_err_accel();
if (ao_sample_tick - ao_sample_prev_tick > 5) {
- ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a;
- ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a;
- ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a;
+ ao_k_height +=(ao_k_t) AO_ACCEL_K0_10 * ao_error_a;
+ ao_k_speed += (ao_k_t) AO_ACCEL_K1_10 * ao_error_a;
+ ao_k_accel += (ao_k_t) AO_ACCEL_K2_10 * ao_error_a;
return;
}
- ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a;
- ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a;
- ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a;
+ ao_k_height += (ao_k_t) AO_ACCEL_K0_100 * ao_error_a;
+ ao_k_speed += (ao_k_t) AO_ACCEL_K1_100 * ao_error_a;
+ ao_k_accel += (ao_k_t) AO_ACCEL_K2_100 * ao_error_a;
}
#endif /* else FORCE_ACCEL */
diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h
index 33cda3eb..da20e067 100644
--- a/src/kernel/ao_log.h
+++ b/src/kernel/ao_log.h
@@ -176,8 +176,8 @@ struct ao_log_record {
int32_t gps_latitude;
int32_t gps_longitude;
struct {
- int16_t altitude;
- uint16_t unused;
+ uint16_t altitude_low;
+ int16_t altitude_high;
} gps_altitude;
struct {
uint16_t svid;
@@ -246,7 +246,7 @@ struct ao_log_mega {
struct {
int32_t latitude; /* 4 */
int32_t longitude; /* 8 */
- int16_t altitude; /* 12 */
+ uint16_t altitude_low; /* 12 */
uint8_t hour; /* 14 */
uint8_t minute; /* 15 */
uint8_t second; /* 16 */
@@ -261,7 +261,8 @@ struct ao_log_mega {
uint8_t hdop; /* 27 */
uint8_t vdop; /* 28 */
uint8_t mode; /* 29 */
- } gps; /* 30 */
+ int16_t altitude_high; /* 30 */
+ } gps; /* 32 */
/* AO_LOG_GPS_SAT */
struct {
uint16_t channels; /* 4 */
@@ -273,6 +274,11 @@ struct ao_log_mega {
} u;
};
+#define AO_LOG_MEGA_GPS_ALTITUDE(l) ((int32_t) ((l)->u.gps.altitude_high << 16) | ((l)->u.gps.altitude_low))
+#define AO_LOG_MEGA_SET_GPS_ALTITUDE(l,a) (((l)->u.gps.mode |= AO_GPS_MODE_ALTITUDE_24), \
+ ((l)->u.gps.altitude_high = (a) >> 16), \
+ (l)->u.gps.altitude_low = (a))
+
struct ao_log_metrum {
char type; /* 0 */
uint8_t csum; /* 1 */
@@ -306,8 +312,9 @@ struct ao_log_metrum {
struct {
int32_t latitude; /* 4 */
int32_t longitude; /* 8 */
- int16_t altitude; /* 12 */
- } gps; /* 14 */
+ uint16_t altitude_low; /* 12 */
+ int16_t altitude_high; /* 14 */
+ } gps; /* 16 */
/* AO_LOG_GPS_TIME */
struct {
uint8_t hour; /* 4 */
@@ -317,7 +324,7 @@ struct ao_log_metrum {
uint8_t year; /* 8 */
uint8_t month; /* 9 */
uint8_t day; /* 10 */
- uint8_t pad; /* 11 */
+ uint8_t pdop; /* 11 */
} gps_time; /* 12 */
/* AO_LOG_GPS_SAT (up to three packets) */
struct {
@@ -381,7 +388,7 @@ struct ao_log_gps {
struct {
int32_t latitude; /* 4 */
int32_t longitude; /* 8 */
- int16_t altitude; /* 12 */
+ uint16_t altitude_low; /* 12 */
uint8_t hour; /* 14 */
uint8_t minute; /* 15 */
uint8_t second; /* 16 */
@@ -396,7 +403,7 @@ struct ao_log_gps {
uint8_t hdop; /* 27 */
uint8_t vdop; /* 28 */
uint8_t mode; /* 29 */
- uint8_t state; /* 30 */
+ int16_t altitude_high; /* 30 */
} gps; /* 31 */
/* AO_LOG_GPS_SAT */
struct {
diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c
index 3b728c25..a5a6358b 100644
--- a/src/kernel/ao_log_gps.c
+++ b/src/kernel/ao_log_gps.c
@@ -75,7 +75,8 @@ ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data)
log.type = AO_LOG_GPS_TIME;
log.u.gps.latitude = gps_data->latitude;
log.u.gps.longitude = gps_data->longitude;
- log.u.gps.altitude = gps_data->altitude;
+ log.u.gps.altitude_low = gps_data->altitude_low;
+ log.u.gps.altitude_high = gps_data->altitude_high;
log.u.gps.hour = gps_data->hour;
log.u.gps.minute = gps_data->minute;
diff --git a/src/kernel/ao_log_gps.h b/src/kernel/ao_log_gps.h
index 5851f4d1..a9e8c831 100644
--- a/src/kernel/ao_log_gps.h
+++ b/src/kernel/ao_log_gps.h
@@ -21,9 +21,6 @@
#ifndef _AO_LOG_GPS_H_
#define _AO_LOG_GPS_H_
-uint8_t
-ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt);
-
void
ao_log_gps_flight(void);
diff --git a/src/kernel/ao_microkalman.c b/src/kernel/ao_microkalman.c
index 0684ea2b..75a29cc4 100644
--- a/src/kernel/ao_microkalman.c
+++ b/src/kernel/ao_microkalman.c
@@ -22,19 +22,19 @@
#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 to_fix_v(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix_k(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 fix8_to_fix_v(x) ((x) << 8)
#define fix16_to_fix8(x) ((x) >> 8)
#include <ao_kalman.h>
/* Basic time step (96ms) */
-#define AO_MK_STEP to_fix16(0.096)
+#define AO_MK_STEP to_fix_v(0.096)
/* step ** 2 / 2 */
-#define AO_MK_STEP_2_2 to_fix16(0.004608)
+#define AO_MK_STEP_2_2 to_fix_v(0.004608)
uint32_t ao_k_pa; /* 24.8 fixed point */
int32_t ao_k_pa_speed; /* 16.16 fixed point */
@@ -49,7 +49,7 @@ ao_microkalman_init(void)
{
ao_pa = pa;
ao_k_pa = pa << 8;
-}
+}
void
ao_microkalman_predict(void)
diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
index 34658951..29bf2bf6 100644
--- a/src/kernel/ao_sample.c
+++ b/src/kernel/ao_sample.c
@@ -245,7 +245,7 @@ ao_sample_preflight(void)
} else {
#if HAS_ACCEL
ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
- ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g;
+ ao_accel_scale = to_fix_32(GRAVITY * 2 * 16) / ao_accel_2g;
#endif
ao_sample_preflight_set();
ao_preflight = FALSE;
diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h
index 16d4c507..2ec998bd 100644
--- a/src/kernel/ao_sample.h
+++ b/src/kernel/ao_sample.h
@@ -24,6 +24,24 @@
* ao_sample.c
*/
+#ifndef AO_VALUE_32
+#define AO_VALUE_32 1
+#endif
+
+#if AO_VALUE_32
+/*
+ * For 32-bit computed values, use 64-bit intermediates.
+ */
+typedef int64_t ao_k_t;
+typedef int32_t ao_v_t;
+#else
+/*
+ * For 16-bit computed values, use 32-bit intermediates.
+ */
+typedef int32_t ao_k_t;
+typedef int16_t ao_v_t;
+#endif
+
/*
* Barometer calibration
*
@@ -87,9 +105,9 @@
* 2047m/s² (over 200g)
*/
-#define AO_M_TO_HEIGHT(m) ((int16_t) (m))
-#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16))
-#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16))
+#define AO_M_TO_HEIGHT(m) ((ao_v_t) (m))
+#define AO_MS_TO_SPEED(ms) ((ao_v_t) ((ms) * 16))
+#define AO_MSS_TO_ACCEL(mss) ((ao_v_t) ((mss) * 16))
extern __pdata uint16_t ao_sample_tick; /* time of last data */
extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */
@@ -134,21 +152,33 @@ uint8_t ao_sample(void);
* ao_kalman.c
*/
-#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
-#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_16(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix_32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_64(x) ((int64_t) ((x) * 65536.0 + 0.5))
+
+#ifdef AO_VALUE_32
+#if AO_VALUE_32
+#define to_fix_v(x) to_fix_32(x)
+#define to_fix_k(x) to_fix_64(x)
+#else
+#define to_fix_v(x) to_fix_16(x)
+#define to_fix_k(x) to_fix_32(x)
+#endif
+
#define from_fix(x) ((x) >> 16)
-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 __xdata int16_t ao_max_height; /* max of ao_height */
-extern __xdata int16_t ao_avg_height; /* running average of height */
+extern __pdata ao_v_t ao_height; /* meters */
+extern __pdata ao_v_t ao_speed; /* m/s * 16 */
+extern __pdata ao_v_t ao_accel; /* m/s² * 16 */
+extern __xdata ao_v_t ao_max_height; /* max of ao_height */
+extern __xdata ao_v_t ao_avg_height; /* running average of height */
-extern __pdata int16_t ao_error_h;
-extern __pdata int16_t ao_error_h_sq_avg;
+extern __pdata ao_v_t ao_error_h;
+extern __pdata ao_v_t ao_error_h_sq_avg;
#if HAS_ACCEL
-extern __pdata int16_t ao_error_a;
+extern __pdata ao_v_t ao_error_a;
+#endif
#endif
void ao_kalman(void);
diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c
index d321c8ff..56bd715e 100644
--- a/src/kernel/ao_telemetry.c
+++ b/src/kernel/ao_telemetry.c
@@ -344,7 +344,7 @@ ao_send_location(void)
ao_mutex_get(&ao_gps_mutex);
ao_xmemcpy(&telemetry.location.flags,
&ao_gps_data.flags,
- 26);
+ 27);
telemetry.location.tick = ao_gps_tick;
ao_mutex_put(&ao_gps_mutex);
ao_radio_send(&telemetry, sizeof (telemetry));
diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h
index be7d0340..83d432cf 100644
--- a/src/kernel/ao_telemetry.h
+++ b/src/kernel/ao_telemetry.h
@@ -86,12 +86,9 @@ struct ao_telemetry_configuration {
#define AO_TELEMETRY_LOCATION 0x05
-#define AO_GPS_MODE_NOT_VALID 'N'
-#define AO_GPS_MODE_AUTONOMOUS 'A'
-#define AO_GPS_MODE_DIFFERENTIAL 'D'
-#define AO_GPS_MODE_ESTIMATED 'E'
-#define AO_GPS_MODE_MANUAL 'M'
-#define AO_GPS_MODE_SIMULATED 'S'
+/* Mode bits */
+
+#define AO_GPS_MODE_ALTITUDE_24 (1 << 0) /* reports 24-bits of altitude */
struct ao_telemetry_location {
uint16_t serial; /* 0 */
@@ -99,7 +96,7 @@ struct ao_telemetry_location {
uint8_t type; /* 4 */
uint8_t flags; /* 5 Number of sats and other flags */
- int16_t altitude; /* 6 GPS reported altitude (m) */
+ uint16_t altitude_low; /* 6 GPS reported altitude (m) */
int32_t latitude; /* 8 latitude (degrees * 10⁷) */
int32_t longitude; /* 12 longitude (degrees * 10⁷) */
uint8_t year; /* 16 (- 2000) */
@@ -115,10 +112,31 @@ struct ao_telemetry_location {
uint16_t ground_speed; /* 26 cm/s */
int16_t climb_rate; /* 28 cm/s */
uint8_t course; /* 30 degrees / 2 */
- uint8_t unused; /* 31 unused */
+ int8_t altitude_high; /* 31 high byte of altitude */
/* 32 */
};
+#if HAS_GPS
+
+#ifndef HAS_WIDE_GPS
+#define HAS_WIDE_GPS 1
+#endif
+
+#if HAS_WIDE_GPS
+typedef int32_t gps_alt_t;
+#define AO_TELEMETRY_LOCATION_ALTITUDE(l) (((gps_alt_t) (l)->altitude_high << 16) | ((l)->altitude_low))
+#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->mode |= AO_GPS_MODE_ALTITUDE_24), \
+ ((l)->altitude_high = (a) >> 16), \
+ ((l)->altitude_low = (a)))
+#else
+typedef int16_t gps_alt_t;
+#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((gps_alt_t) (l)->altitude_low)
+#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->mode = 0, \
+ (l)->altitude_low = (a)))
+#endif /* HAS_WIDE_GPS */
+
+#endif /* HAS_GPS */
+
#define AO_TELEMETRY_SATELLITE 0x06
struct ao_telemetry_satellite_info {
diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c
index 9febc7f0..d9434048 100644
--- a/src/kernel/ao_tracker.c
+++ b/src/kernel/ao_tracker.c
@@ -36,9 +36,9 @@ ao_usb_connected(void)
#endif
struct gps_position {
- int32_t latitude;
- int32_t longitude;
- int16_t altitude;
+ int32_t latitude;
+ int32_t longitude;
+ gps_alt_t altitude;
};
#define GPS_RING 16
@@ -122,12 +122,13 @@ ao_tracker(void)
{
uint8_t ring;
uint8_t moving = 0;
+ gps_alt_t altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data);
for (ring = ao_gps_ring_next(gps_head); ring != gps_head; ring = ao_gps_ring_next(ring)) {
ground_distance = ao_distance(gps_data.latitude, gps_data.longitude,
gps_position[ring].latitude,
gps_position[ring].longitude);
- height = gps_position[ring].altitude - gps_data.altitude;
+ height = gps_position[ring].altitude - altitude;
if (height < 0)
height = -height;
@@ -153,7 +154,7 @@ ao_tracker(void)
ao_log_gps_data(gps_tick, &gps_data);
gps_position[gps_head].latitude = gps_data.latitude;
gps_position[gps_head].longitude = gps_data.longitude;
- gps_position[gps_head].altitude = gps_data.altitude;
+ gps_position[gps_head].altitude = altitude;
gps_head = ao_gps_ring_next(gps_head);
ao_mutex_put(&tracker_mutex);
}
@@ -203,7 +204,7 @@ ao_tracker_set_telem(void)
printf ("log_started: %d\n", log_started);
printf ("latitude: %ld\n", (long) gps_data.latitude);
printf ("longitude: %ld\n", (long) gps_data.longitude);
- printf ("altitude: %d\n", gps_data.altitude);
+ printf ("altitude: %ld\n", AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data));
printf ("log_running: %d\n", ao_log_running);
printf ("log_start_pos: %ld\n", (long) ao_log_start_pos);
printf ("log_cur_pos: %ld\n", (long) ao_log_current_pos);
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-17 19:55:19 +0000