altos: Add bit-bang i2c driver

[fw/altos] / src / kernel / ao_gps_report_mega.c

diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c
index 5e3c71bfa309e1ecb6f3fecb954dde596ff12283..0cba972be07ce35bb353e97eba0a3e7b9a8c174b 100644 (file)
--- a/src/kernel/ao_gps_report_mega.c
+++ b/src/kernel/ao_gps_report_mega.c
@@ -3,7 +3,8 @@
  *
  * 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.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -18,12 +19,49 @@
 #include "ao.h"
 #include "ao_log.h"
 
+#ifndef GPS_SPARSE_LOG
+#define GPS_SPARSE_LOG 0
+#endif
+
+#if GPS_SPARSE_LOG
+static int32_t prev_lat, prev_lon, int16_t prev_alt;
+static uint8_t has_prev, unmoving;
+
+#define GPS_SPARSE_UNMOVING_REPORTS    10
+#define GPS_SPARSE_UNMOVING_GROUND     10
+#define GPS_SPARSE_UNMOVING_AIR                10
+
+static uint8_t
+ao_gps_sparse_should_log(int32_t lat, int32_t lon, int16_t alt)
+{
+       uint8_t ret = 1;
+
+       if (has_prev && ao_log_running) {
+               uint32_t        h = ao_distance(prev_lat, prev_lon, lat, lon);
+               uint16_t        v = alt > prev_alt ? (alt - prev_alt) : (prev_alt - alt);
+
+               if (h < GPS_SPARSE_UNMOVING_GROUND && v < GPS_SPARSE_UNMOVING_AIR) {
+                       if (unmoving < GPS_SPARSE_UNMOVING_REPORTS)
+                               ++unmoving;
+               } else
+                       unmoving = 0;
+       } else
+               unmoving = 0;
+
+       prev_lat = lat;
+       prev_lon = lon;
+       prev_alt = alt;
+       has_prev = 1;
+       return unmoving >= GPS_SPARSE_UNMOVING_REPORTS;
+}
+#endif
+
 void
 ao_gps_report_mega(void)
 {
-       static __xdata struct ao_log_mega               gps_log;
-       static __xdata struct ao_telemetry_location     gps_data;
-       static __xdata struct ao_telemetry_satellite    gps_tracking_data;
+       static struct ao_log_mega               gps_log;
+       static struct ao_telemetry_location     gps_data;
+       static struct ao_telemetry_satellite    gps_tracking_data;
        uint8_t new;
        uint8_t c, n, i;
 
@@ -32,19 +70,27 @@ ao_gps_report_mega(void)
                        ao_sleep(&ao_gps_new);
                ao_mutex_get(&ao_gps_mutex);
                if (new & AO_GPS_NEW_DATA)
-                       ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+                       memcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
                if (new & AO_GPS_NEW_TRACKING)
-                       ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+                       memcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
                ao_gps_new = 0;
                ao_mutex_put(&ao_gps_mutex);
 
+#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,
+                                             AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data))
+                       continue;
+#endif
                if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
+
                        gps_log.tick = ao_gps_tick;
                        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;
@@ -59,7 +105,7 @@ ao_gps_report_mega(void)
                        gps_log.u.gps.hdop = gps_data.hdop;
                        gps_log.u.gps.vdop = gps_data.vdop;
                        gps_log.u.gps.mode = gps_data.mode;
-                       ao_log_mega(&gps_log);
+                       ao_log_write(&gps_log);
                }
                if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels) != 0) {
                        gps_log.tick = ao_gps_tick;
@@ -74,12 +120,12 @@ ao_gps_report_mega(void)
                                                break;
                                }
                        gps_log.u.gps_sat.channels = i;
-                       ao_log_mega(&gps_log);
+                       ao_log_write(&gps_log);
                }
        }
 }
 
-__xdata struct ao_task ao_gps_report_mega_task;
+struct ao_task ao_gps_report_mega_task;
 
 void
 ao_gps_report_mega_init(void)