diff --git a/exercises/25_motioncal_tbeam/README.md b/exercises/25_motioncal_tbeam/README.md new file mode 100644 index 0000000..164c700 --- /dev/null +++ b/exercises/25_motioncal_tbeam/README.md @@ -0,0 +1,57 @@ +# Exercise 25: MotionCal T-Beam Bridge + +Streams the T-Beam Supreme QMC6310 magnetometer in the ASCII format accepted by +Paul Stoffregen's MotionCal desktop tool. + +https://github.com/PaulStoffregen/MotionCal.git (fetch) + +MotionCal expects: + +```text +Raw:accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z +``` + +This exercise only has a magnetometer, so it sends a stationary accelerometer +placeholder and zero gyro: + +```text +Raw:0,0,8192,0,0,0,mag_x,mag_y,mag_z +``` + +The magnetic values are converted from SensorLib Gauss readings into MotionCal's +integer units where 1 count is 0.1 microtesla. + +## Build + +```sh +cd /usr/local/src/microreticulum/microReticulumTbeam/exercises/25_motioncal_tbeam +source /home/jlpoole/rnsenv/bin/activate +pio run +``` + +## Upload + +```sh +source /home/jlpoole/rnsenv/bin/activate +pio run -t upload +``` + +Use a specific board environment if needed: + +```sh +pio run -e guy -t upload +``` + +## MotionCal + +Build and run MotionCal as before: + +```sh +cd /usr/local/src/MotionCal +make WXCONFIG=wx-config LDFLAGS="-lglut -lGLU -lGL -lm" +GDK_BACKEND=x11 ./MotionCal +``` + +Select the T-Beam USB serial port in MotionCal. The firmware also accepts +MotionCal's 68-byte calibration packet and echoes `Cal1:` and `Cal2:` lines so +MotionCal can confirm the send. diff --git a/exercises/25_motioncal_tbeam/platformio.ini b/exercises/25_motioncal_tbeam/platformio.ini new file mode 100644 index 0000000..f004814 --- /dev/null +++ b/exercises/25_motioncal_tbeam/platformio.ini @@ -0,0 +1,77 @@ +; 20260424 Codex +; Exercise 25_motioncal_tbeam + +[platformio] +default_envs = cy + +[env] +platform = espressif32 +framework = arduino +board = esp32-s3-devkitc-1 +board_build.partitions = default_8MB.csv +monitor_speed = 115200 +extra_scripts = pre:scripts/set_build_epoch.py +lib_deps = + Wire + olikraus/U8g2@^2.36.4 + lewisxhe/XPowersLib@0.3.3 + +build_flags = + -I ../../shared/boards + -I ../../external/microReticulum_Firmware + -I ../../../../LilyGo-LoRa-Series/lib/SensorLib/src + -D BOARD_MODEL=BOARD_TBEAM_S_V1 + -D OLED_SDA=17 + -D OLED_SCL=18 + -D OLED_ADDR=0x3C + -D ARDUINO_USB_MODE=1 + -D ARDUINO_USB_CDC_ON_BOOT=1 + +[env:amy] +extends = env +build_flags = + ${env.build_flags} + -D BOARD_ID=\"AMY\" + -D NODE_LABEL=\"Amy\" + +[env:bob] +extends = env +build_flags = + ${env.build_flags} + -D BOARD_ID=\"BOB\" + -D NODE_LABEL=\"Bob\" + +[env:cy] +extends = env +build_flags = + ${env.build_flags} + -D BOARD_ID=\"CY\" + -D NODE_LABEL=\"Cy\" + +[env:dan] +extends = env +build_flags = + ${env.build_flags} + -D BOARD_ID=\"DAN\" + -D NODE_LABEL=\"Dan\" + +[env:ed] +extends = env +build_flags = + ${env.build_flags} + -D BOARD_ID=\"ED\" + -D NODE_LABEL=\"Ed\" + +[env:flo] +extends = env +build_flags = + ${env.build_flags} + -D BOARD_ID=\"FLO\" + -D NODE_LABEL=\"Flo\" + +[env:guy] +extends = env +build_flags = + ${env.build_flags} + -D BOARD_ID=\"GUY\" + -D NODE_LABEL=\"Guy\" diff --git a/exercises/25_motioncal_tbeam/scripts/set_build_epoch.py b/exercises/25_motioncal_tbeam/scripts/set_build_epoch.py new file mode 100644 index 0000000..40ef7ca --- /dev/null +++ b/exercises/25_motioncal_tbeam/scripts/set_build_epoch.py @@ -0,0 +1,12 @@ +import time +Import("env") + +epoch = int(time.time()) +utc_tag = time.strftime("%Y%m%d_%H%M%S_z", time.gmtime(epoch)) + +env.Append( + CPPDEFINES=[ + ("FW_BUILD_EPOCH", str(epoch)), + ("FW_BUILD_UTC", '"%s"' % utc_tag), + ] +) diff --git a/exercises/25_motioncal_tbeam/src/main.cpp b/exercises/25_motioncal_tbeam/src/main.cpp new file mode 100644 index 0000000..60de5ee --- /dev/null +++ b/exercises/25_motioncal_tbeam/src/main.cpp @@ -0,0 +1,348 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "SensorQMC6310.hpp" +#include "tbeam_supreme_adapter.h" + +namespace { + +#ifndef BOARD_ID +#define BOARD_ID "CY" +#endif + +#ifndef NODE_LABEL +#define NODE_LABEL "Cy" +#endif + +#ifndef FW_BUILD_UTC +#define FW_BUILD_UTC unknown +#endif + +#ifndef OLED_SDA +#define OLED_SDA 17 +#endif + +#ifndef OLED_SCL +#define OLED_SCL 18 +#endif + +#ifndef OLED_ADDR +#define OLED_ADDR 0x3C +#endif + +#define STR_INNER(x) #x +#define STR(x) STR_INNER(x) + +static constexpr const char* kExerciseName = "Exercise 25"; +static constexpr const char* kBoardId = BOARD_ID; +static constexpr const char* kNodeLabel = NODE_LABEL; +static constexpr const char* kBuildUtc = STR(FW_BUILD_UTC); +static constexpr uint32_t kSampleIntervalMs = 40; +static constexpr uint32_t kDisplayIntervalMs = 250; +static constexpr uint8_t kMagCandidateCount = 3; +static constexpr uint8_t kMagCandidates[kMagCandidateCount] = {0x1C, 0x3C, 0x2C}; +static constexpr uint16_t kCalibrationPacketSize = 68; +static constexpr uint16_t kCalibrationSignature = 0x5475; +static constexpr size_t kFloatCount = 16; + +XPowersLibInterface* g_pmu = nullptr; +U8G2_SH1106_128X64_NONAME_F_HW_I2C g_oled(U8G2_R0, U8X8_PIN_NONE); +SensorQMC6310 g_qmc; + +bool g_displayReady = false; +bool g_magReady = false; +uint8_t g_magAddress = 0; +uint8_t g_magChipId = 0; +char g_magLabel[16] = "UNKNOWN"; +uint32_t g_lastSampleMs = 0; +uint32_t g_lastDisplayMs = 0; +uint32_t g_sampleCount = 0; +int16_t g_lastMagCounts[3] = {0, 0, 0}; +float g_lastMagUt[3] = {0.0f, 0.0f, 0.0f}; + +uint8_t g_calPacket[kCalibrationPacketSize]; +uint16_t g_calPacketLen = 0; + +uint16_t crc16Update(uint16_t crc, uint8_t data) { + crc ^= data; + for (uint8_t i = 0; i < 8; ++i) { + if ((crc & 1U) != 0U) { + crc = (crc >> 1U) ^ 0xA001U; + } else { + crc >>= 1U; + } + } + return crc; +} + +float readFloatLe(const uint8_t* p) { + union { + uint32_t n; + float f; + } value; + value.n = ((uint32_t)p[0]) | + ((uint32_t)p[1] << 8U) | + ((uint32_t)p[2] << 16U) | + ((uint32_t)p[3] << 24U); + return value.f; +} + +int16_t clampToInt16(long value) { + if (value > INT16_MAX) return INT16_MAX; + if (value < INT16_MIN) return INT16_MIN; + return (int16_t)value; +} + +int16_t microteslaToMotionCalCounts(float uT) { + return clampToInt16(lroundf(uT * 10.0f)); +} + +void drawLines(const char* l1, + const char* l2 = nullptr, + const char* l3 = nullptr, + const char* l4 = nullptr, + const char* l5 = nullptr) { + if (!g_displayReady) { + return; + } + + g_oled.clearBuffer(); + g_oled.setFont(u8g2_font_6x12_tf); + if (l1) g_oled.drawUTF8(0, 12, l1); + if (l2) g_oled.drawUTF8(0, 24, l2); + if (l3) g_oled.drawUTF8(0, 36, l3); + if (l4) g_oled.drawUTF8(0, 48, l4); + if (l5) g_oled.drawUTF8(0, 60, l5); + g_oled.sendBuffer(); +} + +void initDisplay() { + Wire.begin(OLED_SDA, OLED_SCL); + Wire.setClock(400000); + g_oled.setI2CAddress(OLED_ADDR << 1); + g_oled.setBusClock(400000); + g_oled.begin(); + g_oled.setPowerSave(0); + g_displayReady = true; + drawLines(kExerciseName, "MotionCal Bridge", kBoardId, "starting..."); +} + +bool probeI2cAddr(TwoWire& wire, uint8_t addr) { + wire.beginTransmission(addr); + return wire.endTransmission() == 0; +} + +bool detectMagnetometer() { + for (uint8_t i = 0; i < kMagCandidateCount; ++i) { + const uint8_t addr = kMagCandidates[i]; + if (!probeI2cAddr(Wire, addr)) { + continue; + } + + g_magAddress = addr; + if (addr == 0x1C) { + strlcpy(g_magLabel, "QMC6310U", sizeof(g_magLabel)); + } else if (addr == 0x3C) { + strlcpy(g_magLabel, "QMC6310N", sizeof(g_magLabel)); + } else if (addr == 0x2C) { + strlcpy(g_magLabel, "QMC5883P", sizeof(g_magLabel)); + } else { + strlcpy(g_magLabel, "QST-MAG", sizeof(g_magLabel)); + } + return true; + } + return false; +} + +bool initMagnetometer() { + if (!detectMagnetometer()) { + return false; + } + + if (!g_qmc.begin(Wire, g_magAddress, tbeam_supreme::i2cSda(), tbeam_supreme::i2cScl())) { + return false; + } + + g_magChipId = g_qmc.getChipID(); + return g_qmc.configMagnetometer( + OperationMode::CONTINUOUS_MEASUREMENT, + MagFullScaleRange::FS_2G, + 100.0f, + MagOverSampleRatio::OSR_4, + MagDownSampleRatio::DSR_1); +} + +void printBootSummary() { + Serial.printf("exercise=%s MotionCal T-Beam bridge\r\n", kExerciseName); + Serial.printf("board_id=%s node_label=%s build=%s\r\n", kBoardId, kNodeLabel, kBuildUtc); + Serial.printf("serial_format=Raw:accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z\r\n"); + Serial.printf("mag_units=MotionCal integer counts, 1 count = 0.1 uT\r\n"); +} + +void streamMotionCalRaw(const MagnetometerData& data) { + const float xUt = data.magnetic_field.x * 100.0f; + const float yUt = data.magnetic_field.y * 100.0f; + const float zUt = data.magnetic_field.z * 100.0f; + + g_lastMagUt[0] = xUt; + g_lastMagUt[1] = yUt; + g_lastMagUt[2] = zUt; + g_lastMagCounts[0] = microteslaToMotionCalCounts(xUt); + g_lastMagCounts[1] = microteslaToMotionCalCounts(yUt); + g_lastMagCounts[2] = microteslaToMotionCalCounts(zUt); + ++g_sampleCount; + + Serial.printf("Raw:0,0,8192,0,0,0,%d,%d,%d\r\n", + (int)g_lastMagCounts[0], + (int)g_lastMagCounts[1], + (int)g_lastMagCounts[2]); +} + +void updateDisplay() { + char line3[28]; + char line4[28]; + char line5[28]; + + snprintf(line3, sizeof(line3), "%s 0x%02X id 0x%02X", g_magLabel, g_magAddress, g_magChipId); + snprintf(line4, sizeof(line4), "%6.1f %6.1f", g_lastMagUt[0], g_lastMagUt[1]); + snprintf(line5, sizeof(line5), "Z:%6.1f N:%lu", g_lastMagUt[2], (unsigned long)g_sampleCount); + drawLines(kExerciseName, "MotionCal Raw stream", line3, line4, line5); +} + +void printCalibrationEcho(const float* values) { + Serial.printf("Cal1:%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\r\n", + values[0], values[1], values[2], + values[3], values[4], values[5], + values[6], values[7], values[8], values[9]); + Serial.printf("Cal2:%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\r\n", + values[10], values[13], values[14], + values[13], values[11], values[15], + values[14], values[15], values[12]); +} + +void handleCalibrationPacket(const uint8_t* packet) { + const uint16_t signature = (uint16_t)packet[0] | ((uint16_t)packet[1] << 8U); + if (signature != kCalibrationSignature) { + return; + } + + uint16_t crc = 0xFFFF; + for (uint16_t i = 0; i < kCalibrationPacketSize - 2; ++i) { + crc = crc16Update(crc, packet[i]); + } + const uint16_t got = (uint16_t)packet[66] | ((uint16_t)packet[67] << 8U); + if (crc != got) { + Serial.printf("motioncal_calibration_crc=bad expected=0x%04X got=0x%04X\r\n", crc, got); + return; + } + + float values[kFloatCount]; + const uint8_t* p = packet + 2; + for (size_t i = 0; i < kFloatCount; ++i) { + values[i] = readFloatLe(p); + p += 4; + } + + Serial.printf("motioncal_calibration=received hard_iron_uT=%.3f,%.3f,%.3f field_uT=%.3f\r\n", + values[6], values[7], values[8], values[9]); + printCalibrationEcho(values); +} + +void pollSerialInput() { + while (Serial.available() > 0) { + const int c = Serial.read(); + if (c < 0) { + return; + } + + if (g_calPacketLen == 0 && (uint8_t)c != 0x75U) { + continue; + } + + g_calPacket[g_calPacketLen++] = (uint8_t)c; + if (g_calPacketLen == 2 && g_calPacket[1] != 0x54U) { + g_calPacketLen = (g_calPacket[1] == 0x75U) ? 1U : 0U; + if (g_calPacketLen == 1U) { + g_calPacket[0] = 0x75U; + } + continue; + } + + if (g_calPacketLen >= kCalibrationPacketSize) { + handleCalibrationPacket(g_calPacket); + g_calPacketLen = 0; + } + } +} + +void appSetup() { + Serial.begin(115200); + const uint32_t serialWaitStart = millis(); + while (!Serial && (millis() - serialWaitStart) < 4000) { + delay(10); + } + delay(300); + + printBootSummary(); + initDisplay(); + + if (!tbeam_supreme::initPmuForPeripherals(g_pmu, &Serial)) { + Serial.println("pmu_init=failed"); + drawLines(kExerciseName, "PMU init failed", kBoardId, "see serial"); + return; + } + Serial.println("pmu_init=ok"); + + g_magReady = initMagnetometer(); + if (!g_magReady) { + Serial.println("magnetometer_init=failed"); + drawLines(kExerciseName, "MAG init failed", kBoardId, "see serial"); + return; + } + + Serial.printf("magnetometer_init=ok label=%s addr=0x%02X chip=0x%02X\r\n", + g_magLabel, g_magAddress, g_magChipId); + drawLines(kExerciseName, "MotionCal Bridge", g_magLabel, "streaming Raw..."); + g_lastSampleMs = millis(); + g_lastDisplayMs = millis(); +} + +void appLoop() { + pollSerialInput(); + + if (!g_magReady) { + delay(100); + return; + } + + const uint32_t now = millis(); + if ((uint32_t)(now - g_lastSampleMs) >= kSampleIntervalMs) { + g_lastSampleMs = now; + MagnetometerData data; + if (g_qmc.readData(data) && !data.overflow) { + streamMotionCalRaw(data); + } + } + + if ((uint32_t)(now - g_lastDisplayMs) >= kDisplayIntervalMs) { + g_lastDisplayMs = now; + updateDisplay(); + } + + delay(1); +} + +} // namespace + +void setup() { + appSetup(); +} + +void loop() { + appLoop(); +}