Fix reset-cause detection under Optiboot, gate speed samples, widen counters, add stack/IO diagnostics

Reset cause was silently broken: this board always starts via Optiboot, which
reads MCUSR at its own entry, clears only WDRF, and hands the *original*
MCUSR value to the app in register r2 before jumping to address 0 (see
optiboot.c's own comment on this). Reading MCUSR from main() therefore never
reflected the real reset cause. Fixed by reading r2 in a naked .init0
function (the very first code executed, before anything can clobber it),
matching the mechanism Optiboot documents and the Arduino core relies on.
Watchdog-disable/MCUSR-clear stays a separate .init3 step, for the (unused
today) case of a direct ISP flash bypassing the bootloader.

Speed averaging:
- Gate NMEA speed samples on fix validity, fix type and HDOP (reject when
  RMC status is V, no GSA fix yet, or HDOP > 5.00) and additionally cap any
  single sample at 3x the running average, so one bad low-quality fix can't
  distort the whole-session average.
- Log the distance/moving-time average alongside the NMEA-sample average in
  the session summary, so a growing gap between the two is visible.
- Added NULL guards around the VTG/GSA field lookups this touches, since a
  receiver using an older/shorter NMEA dialect (a plain-NMEA module instead
  of the usual MTK one) could otherwise dereference NULL; the existing
  PMTK/GNSS init handshake already no-ops safely against such a receiver.

Counters: rejected_count is now unsigned long (a 16-bit counter incrementing
near 1Hz over a long poor-fix session could realistically overflow).

Diagnostics:
- Stack headroom: the RAM between .bss/.noinit and RAMEND is painted with a
  canary byte at boot (.init1, before the stack pointer is even set up); the
  status line now reports how much of it is still untouched, which can only
  shrink over the session (AVR121 "StackPaint" pattern).
- SD I/O timing: f_write()/f_sync() calls on all three log files are timed;
  the longest one seen since the last status line is reported and reset each
  time. The periodic sync (IVT_SYNC, currently 180s) now also covers
  system_log and gpx_file, not just gps_log, so they aren't only ever synced
  at file-close.
- Receiver firmware version: send $PMTK605 alongside the existing PMTK355
  query at boot; any otherwise-unhandled $PMTK reply (e.g. PMTK705) is now
  logged verbatim, no parser needed. A non-MTK receiver simply never replies,
  same as the existing GNSS/SBAS handshake already degrades on one.
- Also flush a pending coalesced REJ burst on each status line tick and
  immediately once a point is accepted again, not just on a reason change or
  at session-summary time.

gps-test-tool/main.h updated to match (widened counter, new stub
get_uptime_ms/io_mark/log_prefix/log_reject_flush) so the PC test tool keeps
building against the shared gpx.c/nmea.c.

gps-test-tool/main.h

@@ -215,7 +215,7 @@ struct system_s {
     unsigned long int elevation_loss_raw; /* dm; unfiltered, for comparison only */
     unsigned long int alt_max;          /* dm; highest smoothed altitude seen */
     unsigned long int points_written;
-    unsigned int rejected_count;
+    unsigned long int rejected_count;
     unsigned long int speed_accum_x100;
     unsigned long int speed_sample_count;
     unsigned int hdop_x100;
@@ -269,6 +269,13 @@ void add_elevation(float ele_change);
 static inline void log_reject(unsigned char reason) { System.rejected_count++; (void)reason; }
 static inline void log_jump(unsigned char is_alt, float meters) { (void)is_alt; (void)meters; }
 
+/* soft/main.c's SD I/O timing instrumentation isn't meaningful for the PC
+ * build's stdio-based file stubs; no-op it out. */
+static inline unsigned long int get_uptime_ms(void) { return 0; }
+static inline void io_mark(unsigned long int start_ms) { (void)start_ms; }
+static inline void log_prefix(void) {}
+static inline void log_reject_flush(void) {}
+
 /* Stub functions for PC build */
 #ifdef PC_BUILD
 static inline void set_timer(int timer, int val) { (void)timer; (void)val; }

soft/gpx.c

@@ -172,7 +172,13 @@ unsigned char gpx_write(struct location_s *loc, FIL *file) {
 		strcat_P(buf, PSTR("\t\t\t</trkpt>\n"));
 	}
 
-	return f_write(file, buf, strlen(buf), &bw);
+	{
+		unsigned char ret;
+		unsigned long int t0 = get_uptime_ms();
+		ret = f_write(file, buf, strlen(buf), &bw);
+		io_mark(t0);
+		return ret;
+	}
 }
 
 unsigned char gpx_close(FIL *file) {
@@ -276,6 +282,7 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 
 		if (get_flag(CONFFLAG_VERBOSE_LOG))
 			xputs_P(PSTR("ACCEPT\r\n"));
+		log_reject_flush(); /* close out any pending reject burst now that good data has resumed */
 
 		/* Calculate distance and elevation for accepted point */
 		if (gpx.last_distance_point.lat != 0) {

soft/main.c

@@ -20,18 +20,91 @@ time_t utc;					/* current time */
 struct location_s location;
 struct auto_pause_s auto_pause;
 volatile unsigned long int uptime_ms; /* milliseconds since MCU startup, for the system log */
-
-/* Capture the reset cause before it is cleared by ioinit()/wdt_disable(),
- * and disable the watchdog immediately: otherwise a watchdog-triggered
- * reset with a short timeout could loop forever before main() re-enables it. */
+static unsigned int io_max_ms; /* longest single SD f_write()/f_sync() duration since the last status line, ms */
+
+/*
+ * Reset cause: this board is always started via Optiboot, not a fresh MCU
+ * reset straight into main(). Optiboot reads MCUSR at its own entry, clears
+ * only WDRF (to avoid looping on its own watchdog-based re-entry), and passes
+ * the *original* MCUSR value to the application in register r2 before jumping
+ * to address 0 - see optiboot.c ("save the reset flags in the designated
+ * register ... by putting code in .init0 ... to save R2 to a global
+ * variable"). By the time main() runs, the real MCUSR register no longer
+ * reflects the actual reset cause, so it must be read out of r2 instead, and
+ * that has to happen in .init0 - the very first code executed - before
+ * anything else can clobber the register.
+ */
 volatile unsigned char reset_cause __attribute__((section(".noinit")));
-void capture_reset_cause(void) __attribute__((naked, used, section(".init3")));
-void capture_reset_cause(void) {
-	reset_cause = MCUSR;
+void get_mcusr_from_r2(void) __attribute__((naked, used, section(".init0")));
+void get_mcusr_from_r2(void) {
+	__asm__ __volatile__ ("mov %0, r2\n" : "=r" (reset_cause) :);
+}
+
+/* Disable the watchdog as early as possible (in case this image is ever
+ * flashed directly via ISP, bypassing Optiboot, which already turns the
+ * watchdog off itself before jumping to the application): otherwise a
+ * watchdog-triggered reset with a short timeout could loop forever before
+ * main() re-enables it with the timeout it actually wants. */
+void wdt_disable_early(void) __attribute__((naked, used, section(".init3")));
+void wdt_disable_early(void) {
 	MCUSR = 0;
 	wdt_disable();
 }
 
+/*
+ * Stack headroom (AVR121 "StackPaint" pattern): fill all RAM between the end
+ * of .bss/.noinit and the top of RAM with a known byte as early as possible
+ * (.init1, before the stack pointer itself is set up in .init2), then later
+ * count how many bytes of that pattern are still untouched. That count can
+ * only shrink over the session, as the deepest interrupt/call nesting grows,
+ * so it is a running record of the worst-case stack headroom seen so far -
+ * no extra storage needed to remember a "minimum".
+ */
+#define STACK_CANARY 0xc5
+
+extern uint8_t __heap_start;
+extern uint8_t __stack;
+
+void stack_paint(void) __attribute__((naked, used, section(".init1")));
+void stack_paint(void) {
+	__asm__ __volatile__ (
+		"    ldi r30,lo8(__heap_start)\n"
+		"    ldi r31,hi8(__heap_start)\n"
+		"    ldi r24,lo8(0xc5)\n" /* STACK_CANARY */
+		"    ldi r25,hi8(__stack)\n"
+		"    rjmp .Lstackpaint_cmp\n"
+		".Lstackpaint_loop:\n"
+		"    st Z+,r24\n"
+		".Lstackpaint_cmp:\n"
+		"    cpi r30,lo8(__stack)\n"
+		"    cpc r31,r25\n"
+		"    brlo .Lstackpaint_loop\n"
+		"    breq .Lstackpaint_loop\n"
+	);
+}
+
+unsigned int get_stack_free_bytes(void) {
+	const uint8_t *p = &__heap_start;
+	unsigned int count = 0;
+	while (p <= &__stack && *p == STACK_CANARY) {
+		p++;
+		count++;
+	}
+	return count;
+}
+
+void io_mark(unsigned long int start_ms) {
+	unsigned int dt = (unsigned int)(get_uptime_ms() - start_ms);
+	if (dt > io_max_ms)
+		io_max_ms = dt;
+}
+
+unsigned int io_get_and_reset_max(void) {
+	unsigned int v = io_max_ms;
+	io_max_ms = 0;
+	return v;
+}
+
 void start_bootloader(void) {
 	typedef void (*do_reboot_t)(void);
 	const do_reboot_t do_reboot = (do_reboot_t)((FLASHEND - 1023) >> 1);
@@ -194,7 +267,10 @@ void log_put(int c){
 		uart1_put(c);
 	logbuf.buf[logbuf.len++] = c;
 	if (logbuf.len >= LOG_SIZE && (FLAGS & F_FILEOPEN)) {
-		if(!f_write(&system_log, logbuf.buf, logbuf.len, &bw))
+		unsigned long int t0 = get_uptime_ms();
+		unsigned char err = f_write(&system_log, logbuf.buf, logbuf.len, &bw);
+		io_mark(t0);
+		if (!err)
 			logbuf.len = 0;
 	}
 	if (logbuf.len > LOG_SIZE) {
@@ -205,8 +281,7 @@ void log_put(int c){
 static
 void ioinit (void)
 {
-	wdt_enable(WDTO_4S);
-    MCUSR = 0;
+	wdt_enable(WDTO_4S); /* MCUSR was already cleared in wdt_disable_early() (.init3) */
 	POWER_ON_DDR |= POWER_ON;
 	PORTA |= POWER_ON;
 	
@@ -342,6 +417,17 @@ unsigned int get_avg_speed_x100(void) {
 	return System.speed_accum_x100 / System.speed_sample_count;
 }
 
+/* Average speed the "old" way (distance / moving time), kept only so it can
+ * be logged alongside get_avg_speed_x100() in the session summary - the two
+ * should track each other, and a growing gap is worth noticing. */
+unsigned int get_dist_avg_speed_x100(void) {
+	unsigned int moving = get_logging_time();
+	if (!moving)
+		return 0;
+	/* (distance_cm * 3.6) / moving_s, kept as an all-integer x100 fixed-point value */
+	return (unsigned int)((unsigned long int)System.distance * 36UL / (10UL * moving));
+}
+
 time_t get_pause_time(void) {
 	time_t res = System.pause_time;
 	if (System.current_pause_start < System.time_start)
@@ -518,7 +604,11 @@ int main (void)
 			}
 
 			if (FLAGS & F_FILEOPEN) {
-				f_write(&gps_log, Line, len-1, &bw);
+				{
+					unsigned long int t0 = get_uptime_ms();
+					f_write(&gps_log, Line, len-1, &bw);
+					io_mark(t0);
+				}
 				if (bw != len-1) {
 					System.status = STATUS_FILE_WRITE_ERROR;
 					System.global_error |= ERROR_FILE_WRITE;
@@ -526,11 +616,24 @@ int main (void)
 				}
 				if (System.location_valid == LOC_VALID_NEW) { /* a new point */
 					gpx_process_point(&location, &gpx_file);
-					auto_pause_process();				
+					auto_pause_process();
 				}
 				wdt_reset();
 				if (FLAGS & F_SYNC) {
-					if (f_sync(&gps_log)) {
+					/* Sync all three log files together, on this same periodic
+					 * timer (IVT_SYNC), rather than after every write: an SD
+					 * f_sync() can stall for 100ms+ and must not happen on
+					 * every line, or it would stall the NMEA UART reception. */
+					unsigned long int t0 = get_uptime_ms();
+					unsigned char sync_err = f_sync(&gps_log);
+					io_mark(t0);
+					t0 = get_uptime_ms();
+					sync_err |= f_sync(&gpx_file);
+					io_mark(t0);
+					t0 = get_uptime_ms();
+					sync_err |= f_sync(&system_log);
+					io_mark(t0);
+					if (sync_err) {
 						System.status = STATUS_FILE_SYNC_ERROR;
 						System.global_error |= ERROR_FILE_SYNC;
 						break;

soft/main.h

@@ -196,7 +196,7 @@ struct system_s {
 	unsigned long int speed_accum_x100; // sum of instantaneous NMEA speed samples (km/h * 100) while not paused
 	unsigned long int speed_sample_count; // number of samples in speed_accum_x100
 	unsigned long int points_written; // count of accepted/written track points this session
-	unsigned int rejected_count; // count of rejected candidate points this session
+	unsigned long int rejected_count; // count of rejected candidate points this session (unsigned long: a poor-fix session can reject far more than 65535 times)
 	unsigned int hdop_x100; // HDOP * 100, from GSA
 	unsigned char fix_type; // FIX_TYPE_* from GSA
 	unsigned char speed; // km/h
@@ -282,4 +282,15 @@ void reset_counters(void);
 time_t get_pause_time(void);
 unsigned int get_logging_time(void);
 unsigned int get_avg_speed_x100(void);
+unsigned int get_dist_avg_speed_x100(void);
+
+/* SD card I/O timing: longest single f_write()/f_sync() call observed since
+ * the last time the status line reset the counter (io_get_and_reset_max). */
+void io_mark(unsigned long int start_ms);
+unsigned int io_get_and_reset_max(void);
+
+/* Stack headroom: bytes of the stack-painted region between the end of
+ * .bss/.noinit and RAMEND that are still untouched. Falls only, monotonically,
+ * as the deepest call/interrupt nesting grows over the session. */
+unsigned int get_stack_free_bytes(void);
 

soft/nmea.c

@@ -200,23 +200,40 @@ static void gp_gga_parse(const char *str) {
 	location.time = utc; /* parsed from RMC */
 }
 
+/* Speed samples are only trusted from a valid, reasonably accurate fix:
+ * a single sample from a poor fix (e.g. HDOP 16 with only a few satellites)
+ * would otherwise distort the whole-session average by itself. */
+#define SPEED_SAMPLE_HDOP_MAX_X100	500	/* reject samples with HDOP > 5.00 */
+#define SPEED_SAMPLE_CAP_MULT		3	/* also cap any single sample at this multiple of the running average */
+
 static void gp_vtg_parse(const char *str) {
 	const char *p;
 	double speed;
 	unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
+	unsigned char fix_trustworthy = (FLAGS & F_GPSOK) && System.fix_type != FIX_TYPE_UNKNOWN
+		&& System.hdop_x100 <= SPEED_SAMPLE_HDOP_MAX_X100;
 
 	p = gp_col(str, 9);
-	if (*p == 'N') /* Not valid */
+	if (!p || *p == 'N') /* Not valid, or field missing on this receiver's NMEA dialect */
 		return;
 
 	p = gp_col(str, 7); /* speed in km/h */
+	if (!p)
+		return;
 	xatof(&p, &speed);
 	System.speed = speed+0.5;
 
 	/* Average speed: fixed-point accumulation of instantaneous samples,
 	 * excluding time spent paused, instead of dividing distance by time. */
-	if (!paused) {
-		System.speed_accum_x100 += (unsigned long int)(speed*100.0 + 0.5);
+	if (!paused && fix_trustworthy) {
+		unsigned long int sample_x100 = (unsigned long int)(speed*100.0 + 0.5);
+		if (System.speed_sample_count) {
+			unsigned long int avg_x100 = System.speed_accum_x100 / System.speed_sample_count;
+			unsigned long int cap_x100 = avg_x100 * SPEED_SAMPLE_CAP_MULT;
+			if (cap_x100 && sample_x100 > cap_x100)
+				sample_x100 = cap_x100;
+		}
+		System.speed_accum_x100 += sample_x100;
 		System.speed_sample_count++;
 	}
 }
@@ -226,9 +243,9 @@ static void gp_gsa_parse(const char *str) {
 	double hdop;
 
 	p = gp_col(str, 2); /* fix type: 1 no fix, 2 2D, 3 3D */
-	if (*p == '2')
+	if (p && *p == '2')
 		System.fix_type = FIX_TYPE_2D;
-	else if (*p == '3')
+	else if (p && *p == '3')
 		System.fix_type = FIX_TYPE_3D;
 	else
 		System.fix_type = FIX_TYPE_UNKNOWN;
@@ -332,6 +349,14 @@ time_t gps_parse(const char *str) {	/* Get all required data from NMEA sentences
 		pmtk001_parse(str);
 		return 0;
 	}
+	if (!gp_comp(str, PSTR("PMTK"))) {
+		/* Any other PMTK reply (e.g. PMTK705, the firmware release query
+		 * response) - log it verbatim, no need for a dedicated parser. */
+		log_prefix();
+		xputs_P(PSTR("RX "));
+		xputs(str);
+		return 0;
+	}
 	return 0;
 }
 
@@ -344,11 +369,14 @@ void gps_initialize(void) {
 	 * PMTK355: query gnss search mode (will fail if only GPS is supported)
 	 * PMTK353: set gnss search mode (GPS/Galileo/Glonass/Beidou)
 	 * PMTK313: enable SBAS
+	 * PMTK605: query firmware release; the reply (PMTK705) is logged
+	 *          verbatim by the generic PMTK passthrough in gps_parse()
 	 */
 	switch (System.gps_initialized) {
 		case GPS_INIT_NOT_INITIALIZED:
 			xfprintf(uart0_put_wrap, PSTR("$PMTK355*31\r\n"));
 			xputs_P(PSTR("GNSS mode query sent\r\n"));
+			xfprintf(uart0_put_wrap, PSTR("$PMTK605*31\r\n"));
 			System.gps_initialized = GPS_INIT_QUERY_SENT;
 			break;
 		case GPS_INIT_QUERY_SENT:

soft/syslog.c

@@ -189,6 +189,7 @@ void log_status_line(void) {
 	if (batpct > 100)
 		batpct = 100;
 
+	log_reject_flush();
 	log_prefix();
 	xprintf(PSTR("ST v=%.3f bat%%=%d"), (double)System.bat_volt, batpct);
 	if (System.temperature_ok)
@@ -196,10 +197,11 @@ void log_status_line(void) {
 	xprintf(PSTR(" sats=%u hdop=%.2f fix=%u"), (unsigned int)System.satellites_used,
 		(double)System.hdop_x100/100.0, (unsigned int)System.fix_type);
 	xputs_P(System.sbas ? PSTR("D") : PSTR(""));
-	xprintf(PSTR(" dist=%.2f gain=%.1f/raw%.1f loss=%.1f/raw%.1f wr=%lu rej=%u"),
+	xprintf(PSTR(" dist=%.2f gain=%.1f/raw%.1f loss=%.1f/raw%.1f wr=%lu rej=%lu"),
 		(double)System.distance/100.0, (double)System.elevation_gain/10.0, (double)System.elevation_gain_raw/10.0,
 		(double)System.elevation_loss/10.0, (double)System.elevation_loss_raw/10.0,
 		System.points_written, System.rejected_count);
+	xprintf(PSTR(" iomax=%ums stack=%u"), io_get_and_reset_max(), get_stack_free_bytes());
 	if (utc)
 		xprintf(PSTR(" loc=%s"), get_iso_time(utc, 1));
 	xputs_P(PSTR("\r\n"));
@@ -218,6 +220,9 @@ void log_session_summary(unsigned char final) {
 	h = moving/3600; m = (moving/60)%60;
 	xprintf(PSTR("%uh%02um stopped="), h, m);
 	h = stopped/3600; m = (stopped/60)%60;
-	xprintf(PSTR("%uh%02um altmax=%.1f vmin=%.3f rej=%u\r\n"), h, m,
+	xprintf(PSTR("%uh%02um altmax=%.1f vmin=%.3f rej=%lu"), h, m,
 		(double)System.alt_max/10.0, (double)System.bat_volt_min, System.rejected_count);
+	/* Both averages should track each other; a growing gap is worth noticing. */
+	xprintf(PSTR(" avg_speed=%.2f avg_dist=%.2f\r\n"),
+		(double)get_avg_speed_x100()/100.0, (double)get_dist_avg_speed_x100()/100.0);
 }