#define AO_TRACKER_NOT_MOVING 200
static uint8_t ao_tracker_force_telem;
+static uint8_t ao_tracker_force_launch;
#if HAS_USB_CONNECT
static inline uint8_t
#define ao_usb_connected() 1
#endif
+#define STARTUP_AVERAGE 5
+
+static void
+ao_tracker_start_flight(void)
+{
+ struct ao_log_mega log;
+ ao_log_start();
+ log.type = AO_LOG_FLIGHT;
+ log.tick = ao_time();
+#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_roll = ao_ground_roll;
+ log.u.flight.ground_pitch = ao_ground_pitch;
+ log.u.flight.ground_yaw = ao_ground_yaw;
+#endif
+#if HAS_FLIGHT
+ log.u.flight.ground_pres = ao_ground_pres;
+#endif
+ log.u.flight.flight = ao_flight_number;
+ ao_log_mega(&log);
+}
+
static void
ao_tracker(void)
{
uint32_t ground_distance;
int16_t height;
uint16_t speed;
+ int64_t lat_sum = 0, lon_sum = 0;
+ int32_t alt_sum = 0;
+ int nsamples = 0;
+#if HAS_ADC
ao_timer_set_adc_interval(100);
+#endif
+
+ ao_log_scan();
+
+ ao_rdf_set(1);
+
+ ao_telemetry_set_interval(0);
ao_flight_state = ao_flight_startup;
for (;;) {
switch (ao_flight_state) {
case ao_flight_startup:
/* startup to pad when GPS locks */
- ao_flight_state = ao_flight_pad;
- start_latitude = ao_gps_data.longitude;
- start_longitude = ao_gps_data.latitude;
- start_altitude = ao_gps_data.altitude;
+
+ lat_sum += ao_gps_data.latitude;
+ lon_sum += ao_gps_data.longitude;
+ alt_sum += ao_gps_data.altitude;
+
+ if (++nsamples >= STARTUP_AVERAGE) {
+ ao_flight_state = ao_flight_pad;
+ ao_wakeup(&ao_flight_state);
+ start_latitude = lat_sum / nsamples;
+ start_longitude = lon_sum / nsamples;
+ start_altitude = alt_sum / nsamples;
+ }
break;
case ao_flight_pad:
ground_distance = ao_distance(ao_gps_data.latitude,
- start_latitude,
ao_gps_data.longitude,
+ start_latitude,
start_longitude);
height = ao_gps_data.altitude - start_altitude;
if (height < 0)
height = -height;
+
if (ground_distance >= ao_config.tracker_start_horiz ||
- height >= ao_config.tracker_start_vert)
+ height >= ao_config.tracker_start_vert ||
+ ao_tracker_force_launch)
{
ao_flight_state = ao_flight_drogue;
+ ao_wakeup(&ao_flight_state);
ao_log_start();
+ ao_tracker_start_flight();
}
break;
case ao_flight_drogue:
-
/* Modulate data rates based on speed (in cm/s) */
if (ao_gps_data.climb_rate < 0)
speed = -ao_gps_data.climb_rate;
ao_tracker_set_telem(void)
{
ao_cmd_hex();
- if (ao_cmd_status == ao_cmd_success)
- ao_tracker_force_telem = ao_cmd_lex_i;
+ if (ao_cmd_status == ao_cmd_success) {
+ ao_tracker_force_telem = (ao_cmd_lex_i & 1) != 0;
+ ao_tracker_force_launch = (ao_cmd_lex_i & 2) != 0;
+ printf ("force telem %d force launch %d\n", ao_tracker_force_telem, ao_tracker_force_launch);
+ }
}
static const struct ao_cmds ao_tracker_cmds[] = {