Added basic gps powersaving

firmware/inc/gps.h

@@ -43,6 +43,7 @@ struct ubx_nav_sol gps_get_nav_sol();
 struct ubx_nav_timeutc gps_get_nav_timeutc();
 
 uint8_t gps_is_locked(void);
+void gps_set_powersave_auto(void);
 
 void gps_init(void);
 void usart_loopback_test(void);

firmware/inc/ubx_messages.h

@@ -157,6 +157,17 @@ __PACKED__ struct ubx_cfg_prt {
     uint16_t res3;
   } payload;
 };
+/**
+ * UBX CFG RXM Set powersave mode
+ */
+__PACKED__ struct ubx_cfg_rxm {
+  ubx_message_id_t id;
+  enum ubx_packet_state state;
+  struct {
+    uint8_t reserved1;
+    uint8_t lpMode;
+  } payload;
+};
 
 /**
  * UBX Dynamic Platform Model
@@ -182,6 +193,13 @@ enum {
   UBX_GNSS_QZSS		= 5,
   UBX_GNSS_GLONASS	= 6,
 };
+/**
+ * UBX Powersave Modes
+ */
+enum {
+  UBX_POWERSAVE_OFF	= 0,
+  UBX_POWERSAVE_ON	= 1,
+};
 
 /**
  * =============================================================================

firmware/src/gps.c

@@ -73,6 +73,7 @@ volatile struct ubx_cfg_gnss ubx_cfg_gnss       = { .id = (UBX_CFG | (0x3E << 8)
 volatile struct ubx_cfg_nav5 ubx_cfg_nav5	= { .id = (UBX_CFG | (0x24 << 8)) };
 volatile struct ubx_cfg_tp5 ubx_cfg_tp5		= { .id = (UBX_CFG | (0x31 << 8)) };
 volatile struct ubx_cfg_prt ubx_cfg_prt		= { .id = (UBX_CFG | (0x00 << 8)) };
+volatile struct ubx_cfg_rxm ubx_cfg_rxm		= { .id = (UBX_CFG | (0x11 << 8)) };
 volatile struct ubx_nav_posllh ubx_nav_posllh	= { .id = (UBX_NAV | (0x02 << 8)) };
 volatile struct ubx_nav_timeutc ubx_nav_timeutc	= { .id = (UBX_NAV | (0x21 << 8)) };
 volatile struct ubx_nav_sol ubx_nav_sol		= { .id = (UBX_NAV | (0x06 << 8)) };
@@ -86,6 +87,7 @@ volatile ubx_message_t* const ubx_messages[] = {
   (ubx_message_t*)&ubx_cfg_nav5,
   (ubx_message_t*)&ubx_cfg_tp5,
   (ubx_message_t*)&ubx_cfg_prt,
+  (ubx_message_t*)&ubx_cfg_rxm,
   (ubx_message_t*)&ubx_nav_posllh,
   (ubx_message_t*)&ubx_nav_timeutc,
   (ubx_message_t*)&ubx_nav_sol,
@@ -409,6 +411,39 @@ void gps_set_gnss(void)
 		    (uint8_t*)&ubx_cfg_gnss.payload,
 		    4 + (8 * ubx_cfg_gnss.payload.numConfigBlocks));
 }
+/**
+ * Sets the powersave mode
+ */
+void gps_set_powersave(bool powersave_on)
+{
+  ubx_cfg_rxm.payload.lpMode = (powersave_on ? UBX_POWERSAVE_ON : UBX_POWERSAVE_OFF);
+
+  /* Write the new settings */
+  _ubx_send_message((ubx_message_t*)&ubx_cfg_rxm,
+		    (uint8_t*)&ubx_cfg_rxm.payload,
+		    sizeof(ubx_cfg_rxm.payload));
+}
+/**
+ * Sets the powersave mode automatically based on if we're locked.
+ *
+ * Only call after we've updated our position
+ */
+bool _last_gps_is_locked = false;
+void gps_set_powersave_auto(void)
+{
+  bool is_locked = gps_is_locked();
+
+  if (is_locked != _last_gps_is_locked) {
+    if (is_locked) { /* Just locked */
+      gps_set_powersave(true);
+
+    } else { /* Just unlocked */
+      gps_set_powersave(false);
+    }
+
+    _last_gps_is_locked = is_locked;
+  }
+}
 
 /**
  * Init
@@ -457,6 +492,9 @@ void gps_init(void)
   /* Set which GNSS constellation we'd like to use */
   gps_set_gnss();
 
+  /* Exit powersave mode to start */
+  gps_set_powersave(false);
+
   /* Set the timepulse */
   gps_set_timepulse_five(GPS_TIMEPULSE_FREQ);
 }

firmware/src/main.c

@@ -55,6 +55,8 @@
 void xosc_measure_callback(uint32_t result);
 void timepulse_callback(uint32_t sequence);
 
+int32_t _xosc_error = 0;
+
 /**
  * Initialises the status LED
  */
@@ -171,18 +173,18 @@ void output_telemetry_string(enum telemetry_t type)
 
   /* GPS Status */
   struct ubx_nav_sol sol = gps_get_nav_sol();
-  uint8_t lock = sol.payload.gpsFix;
   uint8_t satillite_count = sol.payload.numSV;
 
   /* GPS Position */
-  if (lock == 0x2 || lock == 0x3 || lock == 0x4) {
+  if (gps_is_locked()) {
     struct ubx_nav_posllh pos = gps_get_nav_posllh();
     lat_fmt = (double)pos.payload.lat / 10000000.0;
     lon_fmt = (double)pos.payload.lon / 10000000.0;
     altitude = pos.payload.height / 1000;
   }
 
-
+  /* GPS Powersave */
+  gps_set_powersave_auto();
 
   /**
    * Format
@@ -199,9 +201,9 @@ void output_telemetry_string(enum telemetry_t type)
 
   /* sprintf - full string */
   len += sprintf(telemetry_string + len,
-		"%s,%02u:%02u:%02u,%02.5f,%03.5f,%ld,%u,%.2f,%.1f",
+		"%s,%02u:%02u:%02u,%02.5f,%03.5f,%ld,%u,%.2f,%.1f,%ld",
 		CALLSIGN, hours, minutes, seconds, lat_fmt, lon_fmt,
-		altitude, satillite_count, battery, temperature);
+                 altitude, satillite_count, battery, temperature, _xosc_error);
 
   if (type == TELEMETRY_CONTESTIA) { contestiaize(telemetry_string + dollars); }
 
@@ -299,9 +301,7 @@ void init(void)
 
 void xosc_measure_callback(uint32_t result)
 {
-  int32_t error = result - XOSC_FREQUENCY;
-
-  error++;
+  _xosc_error = result - XOSC_FREQUENCY;
 }
 
 uint8_t telemetry_trigger_flag = 0;
@@ -320,8 +320,6 @@ int main(void)
 
   init();
 
-  measure_xosc(XOSC_MEASURE_TIMEPULSE, xosc_measure_callback);
-
   led_on();
 
   while (1) {
@@ -344,5 +342,8 @@ int main(void)
     output_telemetry_string((telemetry_alternate++ & 1) ?
                             TELEMETRY_CONTESTIA :
                             TELEMETRY_RTTY);
+
+    /* Measure XOSC against gps timepulse */
+    measure_xosc(XOSC_MEASURE_TIMEPULSE, xosc_measure_callback);
   }
 }