diff --git a/exercises/22_compass/scripts/calc_mag_offsets.py b/exercises/22_compass/scripts/calc_mag_offsets.py new file mode 100644 index 0000000..1ec1a3b --- /dev/null +++ b/exercises/22_compass/scripts/calc_mag_offsets.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import argparse +from dataclasses import dataclass +from pathlib import Path +from typing import Iterable + + +@dataclass +class AxisStats: + min_value: int | None = None + max_value: int | None = None + + def update(self, value: int) -> None: + if self.min_value is None or value < self.min_value: + self.min_value = value + if self.max_value is None or value > self.max_value: + self.max_value = value + + @property + def offset(self) -> float: + assert self.min_value is not None and self.max_value is not None + return (self.min_value + self.max_value) / 2.0 + + @property + def span(self) -> int: + assert self.min_value is not None and self.max_value is not None + return self.max_value - self.min_value + + +@dataclass +class FileStats: + sample_count: int + x: AxisStats + y: AxisStats + z: AxisStats + + +def parse_log(path: Path) -> FileStats: + x = AxisStats() + y = AxisStats() + z = AxisStats() + sample_count = 0 + + with path.open("r", encoding="utf-8") as handle: + for raw_line in handle: + line = raw_line.strip() + if not line or line.startswith("#"): + continue + fields = line.split("\t") + if len(fields) < 13: + continue + x.update(int(fields[4])) + y.update(int(fields[5])) + z.update(int(fields[6])) + sample_count += 1 + + if sample_count == 0: + raise ValueError(f"{path}: no samples found") + + return FileStats(sample_count=sample_count, x=x, y=y, z=z) + + +def merge_stats(items: Iterable[FileStats]) -> FileStats: + merged = FileStats(sample_count=0, x=AxisStats(), y=AxisStats(), z=AxisStats()) + for item in items: + merged.sample_count += item.sample_count + merged.x.update(item.x.min_value) + merged.x.update(item.x.max_value) + merged.y.update(item.y.min_value) + merged.y.update(item.y.max_value) + merged.z.update(item.z.min_value) + merged.z.update(item.z.max_value) + return merged + + +def print_stats(label: str, stats: FileStats) -> None: + print(f"{label}:") + print(f" samples={stats.sample_count}") + print( + f" raw_x min={stats.x.min_value} max={stats.x.max_value} " + f"offset={stats.x.offset:.1f} span={stats.x.span}" + ) + print( + f" raw_y min={stats.y.min_value} max={stats.y.max_value} " + f"offset={stats.y.offset:.1f} span={stats.y.span}" + ) + print( + f" raw_z min={stats.z.min_value} max={stats.z.max_value} " + f"offset={stats.z.offset:.1f} span={stats.z.span}" + ) + + +def main() -> int: + parser = argparse.ArgumentParser(description="Compute raw magnetometer midpoint offsets from one or more log files.") + parser.add_argument("logs", nargs="+", help="Magnetometer log file(s)") + parser.add_argument("--prior-x", type=float, default=0.0, help="Previously compiled raw X offset") + parser.add_argument("--prior-y", type=float, default=0.0, help="Previously compiled raw Y offset") + parser.add_argument("--prior-z", type=float, default=0.0, help="Previously compiled raw Z offset") + args = parser.parse_args() + + per_file: list[tuple[Path, FileStats]] = [] + for item in args.logs: + path = Path(item) + per_file.append((path, parse_log(path))) + + for path, stats in per_file: + print_stats(path.name, stats) + + if len(per_file) > 1: + print() + + combined = merge_stats(stats for _, stats in per_file) + print_stats("combined", combined) + + print() + print("residual_offsets:") + print(f" x={combined.x.offset:.1f}") + print(f" y={combined.y.offset:.1f}") + print(f" z={combined.z.offset:.1f}") + + print() + print("updated_total_offsets:") + print(f" x={args.prior_x + combined.x.offset:.1f}") + print(f" y={args.prior_y + combined.y.offset:.1f}") + print(f" z={args.prior_z + combined.z.offset:.1f}") + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/exercises/22_compass/src/main.cpp b/exercises/22_compass/src/main.cpp index 36ee1b6..bd8b645 100644 --- a/exercises/22_compass/src/main.cpp +++ b/exercises/22_compass/src/main.cpp @@ -61,6 +61,9 @@ static constexpr uint8_t kMagCandidateCount = 3; static constexpr uint8_t kMagCandidates[kMagCandidateCount] = {0x1C, 0x3C, 0x0D}; static constexpr float kDeclinationDeg = MAG_DECLINATION_DEG; static constexpr float kDegPerRad = 57.29577951308232f; +static constexpr float kMagOffsetRawX = -7993.0f; +static constexpr float kMagOffsetRawY = 4608.0f; +static constexpr float kMagOffsetRawZ = 3456.0f; struct ClockDateTime { @@ -385,32 +388,6 @@ void handleWebDownload() { file.close(); } -void startWebServerOLD() { - // GPSQA-CY is a carry-over from previous exercises, but we can keep the SSID for continuity. - //The unique board ID is in the suffix, and the IP address is fixed based on LOG_AP_IP_OCTET. - snprintf(g_apSsid, sizeof(g_apSsid), "GPSQA-%s", kBoardId); - WiFi.mode(WIFI_AP); - WiFi.setSleep(false); - const IPAddress ip(192, 168, LOG_AP_IP_OCTET, 1); - const IPAddress gw(192, 168, LOG_AP_IP_OCTET, 1); - const IPAddress nm(255, 255, 255, 0); - WiFi.softAPConfig(ip, gw, nm); - // no password required. - if (!WiFi.softAP(g_apSsid)) { - Serial.println("wifi_ap=failed"); - return; - } - Serial.println("wifi_ap=started"); - g_server.on("/", HTTP_GET, handleWebIndex); - g_server.on("/files", HTTP_GET, handleWebFiles); - g_server.on("/download", HTTP_GET, handleWebDownload); - g_server.begin(); - g_webReady = true; - - Serial.printf("wifi_ap_ssid=%s\n", g_apSsid); - Serial.printf("wifi_ap_url=http://192.168.%u.1/\n", (unsigned)LOG_AP_IP_OCTET); -} - void startWebServer() { // GPSQA-CY is a carry-over from previous exercises, but we can keep the SSID for continuity. //The unique board ID is in the suffix, and the IP address is fixed based on LOG_AP_IP_OCTET. @@ -518,7 +495,12 @@ bool initMagnetometer() { SensorQMC6310::DATARATE_200HZ, SensorQMC6310::OSR_1, SensorQMC6310::DSR_1); - return rc == 0; + if (rc != 0) { + return false; + } + + g_qmc.setOffset(kMagOffsetRawX, kMagOffsetRawY, kMagOffsetRawZ); + return true; } bool mountSd() { @@ -699,6 +681,7 @@ void printBootSummary() { Serial.printf("oled_wire_pins=sda:%d scl:%d addr:0x%02X\n", OLED_SDA, OLED_SCL, OLED_ADDR); Serial.printf("declination_deg=%.2f\n", kDeclinationDeg); Serial.printf("sample_interval_ms=%lu\n", (unsigned long)kSampleIntervalMs); + Serial.printf("mag_offsets_raw=%.1f,%.1f,%.1f\n", kMagOffsetRawX, kMagOffsetRawY, kMagOffsetRawZ); } void appSetup() { @@ -747,6 +730,7 @@ void appSetup() { } Serial.printf("magnetometer_init=ok label=%s addr=0x%02X chip=0x%02X\n", g_magLabel, g_magAddress, g_magChipId); + Serial.printf("magnetometer_offsets_applied=%.1f,%.1f,%.1f\n", kMagOffsetRawX, kMagOffsetRawY, kMagOffsetRawZ); if (g_timeValid && g_sdMounted) { if (openLogFile()) { @@ -762,7 +746,7 @@ void appSetup() { startWebServer(); - drawLines("Exercise 22", "Magnetometer", g_magLabel, "rotate slowly", "logging @200ms"); + drawLines("Exercise 22", "Magnetometer", g_magLabel, "offsets applied", "logging @200ms"); delay(kUiSplashMs); g_lastSampleMs = millis(); g_lastDisplayMs = millis();