summaryrefslogtreecommitdiff
path: root/src/core
diff options
context:
space:
mode:
Diffstat (limited to 'src/core')
-rw-r--r--src/core/ao.h12
-rw-r--r--src/core/ao_config.c29
-rw-r--r--src/core/ao_data.h21
-rw-r--r--src/core/ao_log.h14
-rw-r--r--src/core/ao_log_mega.c8
-rw-r--r--src/core/ao_sample.c97
-rw-r--r--src/core/ao_sample.h8
-rw-r--r--src/core/ao_sqrt.c2
-rw-r--r--src/core/ao_telemetry.c47
9 files changed, 220 insertions, 18 deletions
diff --git a/src/core/ao.h b/src/core/ao.h
index 54018b37..df5bbf48 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -529,6 +529,11 @@ ao_radio_recv_abort(void);
void
ao_radio_test(uint8_t on);
+typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len);
+
+void
+ao_radio_send_lots(ao_radio_fill_func fill);
+
/*
* Compute the packet length as follows:
*
@@ -679,7 +684,7 @@ extern __xdata uint8_t ao_force_freq;
#endif
#define AO_CONFIG_MAJOR 1
-#define AO_CONFIG_MINOR 12
+#define AO_CONFIG_MINOR 13
#define AO_AES_LEN 16
@@ -706,12 +711,17 @@ struct ao_config {
#if AO_PYRO_NUM
struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */
#endif
+ uint16_t aprs_interval; /* minor version 13 */
};
#define AO_IGNITE_MODE_DUAL 0
#define AO_IGNITE_MODE_APOGEE 1
#define AO_IGNITE_MODE_MAIN 2
+#define AO_RADIO_ENABLE_CORE 1
+#define AO_RADIO_DISABLE_TELEMETRY 2
+#define AO_RADIO_DISABLE_RDF 4
+
#define AO_PAD_ORIENTATION_ANTENNA_UP 0
#define AO_PAD_ORIENTATION_ANTENNA_DOWN 1
diff --git a/src/core/ao_config.c b/src/core/ao_config.c
index 797fe7ec..0aac16a6 100644
--- a/src/core/ao_config.c
+++ b/src/core/ao_config.c
@@ -128,7 +128,7 @@ _ao_config_get(void)
if (minor < 6)
ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION;
if (minor < 8)
- ao_config.radio_enable = TRUE;
+ ao_config.radio_enable = AO_RADIO_ENABLE_CORE;
if (minor < 9)
ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN);
if (minor < 10)
@@ -139,6 +139,8 @@ _ao_config_get(void)
if (minor < 12)
memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro));
#endif
+ if (minor < 13)
+ ao_config.aprs_interval = 0;
ao_config.minor = AO_CONFIG_MINOR;
ao_config_dirty = 1;
}
@@ -498,6 +500,27 @@ ao_config_key_set(void) __reentrant
}
#endif
+#if HAS_APRS
+
+void
+ao_config_aprs_show(void)
+{
+ printf ("APRS interval: %d\n", ao_config.aprs_interval);
+}
+
+void
+ao_config_aprs_set(void)
+{
+ ao_cmd_decimal();
+ if (ao_cmd_status != ao_cmd_success)
+ return;
+ _ao_config_edit_start();
+ ao_config.aprs_interval = ao_cmd_lex_i;
+ _ao_config_edit_finish();
+}
+
+#endif /* HAS_APRS */
+
struct ao_config_var {
__code char *str;
void (*set)(void) __reentrant;
@@ -554,6 +577,10 @@ __code struct ao_config_var ao_config_vars[] = {
{ "P <n,?>\0Configure pyro channels",
ao_pyro_set, ao_pyro_show },
#endif
+#if HAS_APRS
+ { "A <secs>\0APRS packet interval (0 disable)",
+ ao_config_aprs_set, ao_config_aprs_show },
+#endif
{ "s\0Show",
ao_config_show, 0 },
#if HAS_EEPROM
diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index 6fdd19cb..7e2f85d8 100644
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -288,4 +288,25 @@ typedef int16_t accel_t;
#endif
+#if !HAS_GYRO && HAS_MPU6000
+
+#define HAS_GYRO 1
+
+typedef int16_t gyro_t;
+typedef int32_t angle_t;
+
+/* Y axis is aligned with the direction of motion (along) */
+/* X axis is aligned in the other board axis (across) */
+/* Z axis is aligned perpendicular to the board (through) */
+
+#define ao_data_along(packet) ((packet)->mpu6000.accel_y)
+#define ao_data_across(packet) ((packet)->mpu6000.accel_x)
+#define ao_data_through(packet) ((packet)->mpu6000.accel_z)
+
+#define ao_data_roll(packet) ((packet)->mpu6000.gyro_y)
+#define ao_data_pitch(packet) ((packet)->mpu6000.gyro_x)
+#define ao_data_yaw(packet) ((packet)->mpu6000.gyro_z)
+
+#endif
+
#endif /* _AO_DATA_H_ */
diff --git a/src/core/ao_log.h b/src/core/ao_log.h
index 4eaed420..036d6f2d 100644
--- a/src/core/ao_log.h
+++ b/src/core/ao_log.h
@@ -199,10 +199,16 @@ struct ao_log_mega {
union { /* 4 */
/* AO_LOG_FLIGHT */
struct {
- uint16_t flight; /* 4 */
- int16_t ground_accel; /* 6 */
- uint32_t ground_pres; /* 8 */
- } flight; /* 12 */
+ uint16_t flight; /* 4 */
+ int16_t ground_accel; /* 6 */
+ uint32_t ground_pres; /* 8 */
+ int16_t ground_accel_along; /* 16 */
+ int16_t ground_accel_across; /* 12 */
+ int16_t ground_accel_through; /* 14 */
+ int16_t ground_roll; /* 18 */
+ int16_t ground_pitch; /* 20 */
+ int16_t ground_yaw; /* 22 */
+ } flight; /* 24 */
/* AO_LOG_STATE */
struct {
uint16_t state;
diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c
index ac1590db..e03687ad 100644
--- a/src/core/ao_log_mega.c
+++ b/src/core/ao_log_mega.c
@@ -95,6 +95,14 @@ ao_log(void)
#if HAS_ACCEL
log.u.flight.ground_accel = ao_ground_accel;
#endif
+#if HAS_GYRO
+ log.u.flight.ground_accel_along = ao_ground_accel_along;
+ log.u.flight.ground_accel_across = ao_ground_accel_across;
+ log.u.flight.ground_accel_through = ao_ground_accel_through;
+ log.u.flight.ground_pitch = ao_ground_pitch;
+ log.u.flight.ground_yaw = ao_ground_yaw;
+ log.u.flight.ground_roll = ao_ground_roll;
+#endif
log.u.flight.ground_pres = ao_ground_pres;
log.u.flight.flight = ao_flight_number;
ao_log_mega(&log);
diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index 985c0940..dec44f9f 100644
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -37,6 +37,16 @@ __pdata alt_t ao_sample_height;
#if HAS_ACCEL
__pdata accel_t ao_sample_accel;
#endif
+#if HAS_GYRO
+__pdata accel_t ao_sample_accel_along;
+__pdata accel_t ao_sample_accel_across;
+__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;
+#endif
__data uint8_t ao_sample_data;
@@ -53,6 +63,15 @@ __pdata accel_t ao_accel_2g; /* factory accel calibration */
__pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */
#endif
+#if HAS_GYRO
+__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;
+#endif
+
static __pdata uint8_t ao_preflight; /* in preflight mode */
static __pdata uint16_t nsamples;
@@ -60,6 +79,14 @@ __pdata int32_t ao_sample_pres_sum;
#if HAS_ACCEL
__pdata int32_t ao_sample_accel_sum;
#endif
+#if HAS_GYRO
+__pdata int32_t ao_sample_accel_along_sum;
+__pdata int32_t ao_sample_accel_across_sum;
+__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;
+#endif
static void
ao_sample_preflight_add(void)
@@ -68,6 +95,14 @@ ao_sample_preflight_add(void)
ao_sample_accel_sum += ao_sample_accel;
#endif
ao_sample_pres_sum += ao_sample_pres;
+#if HAS_GYRO
+ ao_sample_accel_along_sum += ao_sample_accel_along;
+ ao_sample_accel_across_sum += ao_sample_accel_across;
+ ao_sample_accel_through_sum += ao_sample_accel_through;
+ ao_sample_pitch_sum += ao_sample_pitch;
+ ao_sample_yaw_sum += ao_sample_yaw;
+ ao_sample_roll_sum += ao_sample_roll;
+#endif
++nsamples;
}
@@ -80,8 +115,23 @@ ao_sample_preflight_set(void)
#endif
ao_ground_pres = ao_sample_pres_sum >> 9;
ao_ground_height = pres_to_altitude(ao_ground_pres);
- nsamples = 0;
ao_sample_pres_sum = 0;
+#if HAS_GYRO
+ 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_sample_accel_along_sum = 0;
+ ao_sample_accel_across_sum = 0;
+ ao_sample_accel_through_sum = 0;
+ ao_sample_pitch_sum = 0;
+ ao_sample_yaw_sum = 0;
+ ao_sample_roll_sum = 0;
+ ao_sample_angle = 0;
+#endif
+ nsamples = 0;
}
static void
@@ -122,6 +172,25 @@ ao_sample_preflight_update(void)
ao_sample_preflight_set();
}
+#if 0
+#if HAS_GYRO
+static int32_t p_filt;
+static int32_t y_filt;
+
+static gyro_t inline ao_gyro(void) {
+ gyro_t p = ao_sample_pitch - ao_ground_pitch;
+ gyro_t y = ao_sample_yaw - ao_ground_yaw;
+
+ p_filt = p_filt - (p_filt >> 6) + p;
+ y_filt = y_filt - (y_filt >> 6) + y;
+
+ p = p_filt >> 6;
+ y = y_filt >> 6;
+ return ao_sqrt(p*p + y*y);
+}
+#endif
+#endif
+
uint8_t
ao_sample(void)
{
@@ -147,6 +216,14 @@ ao_sample(void)
ao_sample_accel = ao_data_accel_invert(ao_sample_accel);
ao_data_set_accel(ao_data, ao_sample_accel);
#endif
+#if HAS_GYRO
+ ao_sample_accel_along = ao_data_along(ao_data);
+ ao_sample_accel_across = ao_data_across(ao_data);
+ ao_sample_accel_through = ao_data_through(ao_data);
+ ao_sample_pitch = ao_data_pitch(ao_data);
+ ao_sample_yaw = ao_data_yaw(ao_data);
+ ao_sample_roll = ao_data_roll(ao_data);
+#endif
if (ao_preflight)
ao_sample_preflight();
@@ -154,6 +231,9 @@ ao_sample(void)
if (ao_flight_state < ao_flight_boost)
ao_sample_preflight_update();
ao_kalman();
+#if HAS_GYRO
+ /* do quaternion stuff here... */
+#endif
}
ao_sample_data = ao_data_ring_next(ao_sample_data);
}
@@ -171,6 +251,21 @@ ao_sample_init(void)
ao_sample_accel_sum = 0;
ao_sample_accel = 0;
#endif
+#if HAS_GYRO
+ ao_sample_accel_along_sum = 0;
+ ao_sample_accel_across_sum = 0;
+ ao_sample_accel_through_sum = 0;
+ ao_sample_accel_along = 0;
+ ao_sample_accel_across = 0;
+ ao_sample_accel_through = 0;
+ ao_sample_pitch_sum = 0;
+ ao_sample_yaw_sum = 0;
+ ao_sample_roll_sum = 0;
+ ao_sample_pitch = 0;
+ ao_sample_yaw = 0;
+ ao_sample_roll = 0;
+ ao_sample_angle = 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 9336bdf9..a2dac979 100644
--- a/src/core/ao_sample.h
+++ b/src/core/ao_sample.h
@@ -111,6 +111,14 @@ extern __pdata accel_t ao_ground_accel; /* startup acceleration */
extern __pdata accel_t ao_accel_2g; /* factory accel calibration */
extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */
#endif
+#if HAS_GYRO
+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;
+#endif
void ao_sample_init(void);
diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c
index 09c2e319..3a550eaa 100644
--- a/src/core/ao_sqrt.c
+++ b/src/core/ao_sqrt.c
@@ -15,7 +15,9 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
+#ifndef AO_FLIGHT_TEST
#include "ao.h"
+#endif
/* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */
/**
diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index 52ac9489..8d440e15 100644
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -22,6 +22,12 @@ static __pdata uint16_t ao_telemetry_interval;
static __pdata uint8_t ao_rdf = 0;
static __pdata uint16_t ao_rdf_time;
+#if HAS_APRS
+static __pdata uint16_t ao_aprs_time;
+
+#include <ao_aprs.h>
+#endif
+
#if defined(MEGAMETRUM)
#define AO_SEND_MEGA 1
#endif
@@ -288,30 +294,40 @@ ao_telemetry(void)
while (ao_telemetry_interval == 0)
ao_sleep(&telemetry);
time = ao_rdf_time = ao_time();
+#if HAS_APRS
+ ao_aprs_time = time;
+#endif
while (ao_telemetry_interval) {
-
+#if HAS_APRS
+ if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY))
+#endif
+ {
#ifdef AO_SEND_ALL_BARO
- ao_send_baro();
+ ao_send_baro();
#endif
#ifdef AO_SEND_MEGA
- ao_send_mega_sensor();
- ao_send_mega_data();
+ ao_send_mega_sensor();
+ ao_send_mega_data();
#else
- ao_send_sensor();
+ ao_send_sensor();
#endif
#if HAS_COMPANION
- if (ao_companion_running)
- ao_send_companion();
+ if (ao_companion_running)
+ ao_send_companion();
#endif
- ao_send_configuration();
+ ao_send_configuration();
#if HAS_GPS
- ao_send_location();
- ao_send_satellite();
+ ao_send_location();
+ ao_send_satellite();
#endif
+ }
#ifndef AO_SEND_ALL_BARO
if (ao_rdf &&
+#if HAS_APRS
+ !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) &&
+#endif
(int16_t) (ao_time() - ao_rdf_time) >= 0)
{
#if HAS_IGNITE_REPORT
@@ -325,6 +341,14 @@ ao_telemetry(void)
#endif
ao_radio_rdf();
}
+#if HAS_APRS
+ if (ao_config.aprs_interval != 0 &&
+ (int16_t) (ao_time() - ao_aprs_time) >= 0)
+ {
+ ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval);
+ ao_aprs_send();
+ }
+#endif
#endif
time += ao_telemetry_interval;
delay = time - ao_time();
@@ -389,8 +413,9 @@ ao_rdf_set(uint8_t rdf)
ao_rdf = rdf;
if (rdf == 0)
ao_radio_rdf_abort();
- else
+ else {
ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
+ }
}
__xdata struct ao_task ao_telemetry_task;
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-18 09:06:15 +0000