Notifications
Clear all
Topic starter
25/03/2026 2:21 am
Hi All,
We are using the Teensy 4.1 as a logger (Adafruit GPS is linked with the Teensy) for the MFAM SX. Below is the code for the Arduino IDE (Teensy), for people who might find it useful,
Roi
#include <NativeEthernet.h>
#include <SD.h>
// ---- NETWORK CONFIGURATION ----
byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
IPAddress ip(192, 168, 2, 10);
IPAddress mfamIP(192, 168, 2, 2);
uint16_t mfamPort = 1000;
EthernetClient client;
// ---- PACKET STRUCTURE ----
const int PACKET_SIZE = 1380;
const int SAMPLE_SIZE = 32;
const int HEADER_SIZE = 16;
const int NUM_SAMPLES = 40;
const int SD_CHIP_SELECT = BUILTIN_SDCARD;
uint8_t buffer[PACKET_SIZE];
int bufferPos = 0;
// ---- SD CARD ----
File logFile;
bool sdReady = false;
unsigned long sampleCount = 0;
unsigned long fileStartTime = 0;
char filename[32];
// ---- AUXILIARY CHANNEL STORAGE ----
double gyroX = 0, gyroY = 0, gyroZ = 0, gyroT = 0;
double accelX = 0, accelY = 0, accelZ = 0, accelT = 0;
double compassX = 0, compassY = 0, compassZ = 0, compassT = 0;
// ---- GPS FROM ADAFRUIT MODULE ON SERIAL1 (Pin 0 = RX) ----
char gpsBuffer[256];
int gpsBufferPos = 0;
char gpsString[128] = "";
char gpsDate[12] = "00/00/00";
char gpsTime[16] = "00:00:00.000";
bool gpsFix = false;
uint8_t tsStatus = 0;
// ---- OUTPUT CONTROL ----
// Set to 1 to log every sample, 10 for 100Hz, 20 for 50Hz, etc.
const int DOWNSAMPLE_FACTOR = 1; // 50 Hz output
int downsampleCounter = 0;
// How many minutes per file. Set to 10, 20, 60, etc.
const int FILE_MINUTES = 10;
// ---- LED INDICATOR ----
// Off = starting up
// Very slow blink (every 3 sec) = connected and logging
// Fast blink (4/sec) = connected but no data arriving
// Solid on = no SD card
// 3 quick flashes then pause = cannot connect to MFAM
const int LED_PIN = 13;
unsigned long lastBlinkTime = 0;
bool ledState = false;
unsigned long lastDataTime = 0;
// ---- FUNCTION PROTOTYPES ----
void createNewFile();
void parsePacket(uint8_t* pkt);
void parseAuxChannels(uint8_t* sample, uint16_t frameID);
void readGPS();
void parseGPRMC(char* sentence);
void writeSample(unsigned long timestamp, uint16_t fiducial, double mag1, uint16_t mag1s,
double mag2, uint16_t mag2s, uint16_t sysStatus);
int16_t toSigned16(uint16_t val);
void createNewFile() {
static int fileNumber = 0;
// On first call, find the next available file number
if (fileNumber == 0) {
char testName[32];
for (int i = 1; i <= 99999; i++) {
snprintf(testName, sizeof(testName), "MFAM_%05d.txt", i);
if (!SD.exists(testName)) {
fileNumber = i - 1; // Will be incremented below
break;
}
}
}
fileNumber++;
snprintf(filename, sizeof(filename), "MFAM_%05d.txt", fileNumber);
logFile = SD.open(filename, FILE_WRITE);
if (logFile) {
logFile.println("Mag 1,Mag 2,Fid,SysS,Mg1S,Mg2S,Gyro X,Gyro Y,Gyro Z,Gyro T,Accel X,Accel Y,Accel Z,Accel T,CompassX,CompassY,CompassZ,Comp T,Date,Time,TS Status,GPS");
logFile.flush();
fileStartTime = millis();
Serial.print("Logging to: ");
Serial.println(filename);
} else {
Serial.print("ERROR: Could not create ");
Serial.println(filename);
}
}
void setup() {
Serial.begin(115200);
delay(2000);
// Start GPS serial port (Adafruit Ultimate GPS defaults to 9600 baud)
Serial1.begin(9600);
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, LOW);
memset(gpsString, 0, sizeof(gpsString));
// Initialize SD card
if (SD.begin(SD_CHIP_SELECT)) {
sdReady = true;
Serial.println("SD card ready.");
} else {
Serial.println("WARNING: No SD card found. Serial output only.");
digitalWrite(LED_PIN, HIGH);
}
// Initialize Ethernet
Ethernet.begin(mac, ip);
if (Ethernet.hardwareStatus() == EthernetNoHardware) {
Serial.println("ERROR: No Ethernet hardware found!");
while (true) {}
}
Serial.print("Teensy IP: ");
Serial.println(Ethernet.localIP());
Serial.print("Connecting to MFAM at ");
Serial.print(mfamIP);
Serial.print(":");
Serial.println(mfamPort);
if (client.connect(mfamIP, mfamPort)) {
Serial.println("Connected to MFAM!");
} else {
Serial.println("Connection failed!");
}
Serial.println("Waiting for GPS fix...");
if (sdReady) {
createNewFile();
}
Serial.println("Mag 1,Mag 2,Fid,SysS,Mg1S,Mg2S,Gyro X,Gyro Y,Gyro Z,Gyro T,Accel X,Accel Y,Accel Z,Accel T,CompassX,CompassY,CompassZ,Comp T,Date,Time,TS Status,GPS");
}
void loop() {
// Always read GPS data from Serial1
readGPS();
if (!client.connected()) {
Serial.println("Disconnected. Reconnecting...");
if (sdReady && logFile) {
logFile.flush();
}
for (int i = 0; i < 3; i++) {
digitalWrite(LED_PIN, HIGH);
delay(100);
digitalWrite(LED_PIN, LOW);
delay(100);
}
delay(1400);
client.connect(mfamIP, mfamPort);
if (client.connected()) {
lastDataTime = millis();
}
return;
}
while (client.available()) {
buffer[bufferPos] = client.read();
bufferPos++;
if (bufferPos >= PACKET_SIZE) {
parsePacket(buffer);
bufferPos = 0;
lastDataTime = millis();
}
}
// LED patterns
if (sdReady) {
if (millis() - lastDataTime > 3000) {
if (millis() - lastBlinkTime > 125) {
ledState = !ledState;
digitalWrite(LED_PIN, ledState ? HIGH : LOW);
lastBlinkTime = millis();
}
} else {
if (millis() - lastBlinkTime > 1500) {
ledState = !ledState;
digitalWrite(LED_PIN, ledState ? HIGH : LOW);
lastBlinkTime = millis();
}
}
}
// New file every FILE_MINUTES minutes
if (sdReady && logFile && (millis() - fileStartTime > (unsigned long)FILE_MINUTES * 60UL * 1000UL)) {
logFile.close();
createNewFile();
}
}
// ---- GPS READING FROM ADAFRUIT MODULE ON SERIAL1 ----
void readGPS() {
while (Serial1.available()) {
char c = Serial1.read();
if (c == '$') {
gpsBufferPos = 0;
}
if (gpsBufferPos < (int)sizeof(gpsBuffer) - 1) {
gpsBuffer[gpsBufferPos] = c;
gpsBufferPos++;
}
if (c == '\n' || c == '\r') {
gpsBuffer[gpsBufferPos] = '\0';
if (strncmp(gpsBuffer, "$GPRMC", 6) == 0 || strncmp(gpsBuffer, "$GNRMC", 6) == 0) {
// Save full sentence for logging
strncpy(gpsString, gpsBuffer, sizeof(gpsString) - 1);
gpsString[sizeof(gpsString) - 1] = '\0';
// Remove trailing newline/carriage return
int slen = strlen(gpsString);
while (slen > 0 && (gpsString[slen - 1] == '\n' || gpsString[slen - 1] == '\r')) {
gpsString[slen - 1] = '\0';
slen--;
}
parseGPRMC(gpsBuffer);
}
gpsBufferPos = 0;
}
}
}
void parseGPRMC(char* sentence) {
// $GPRMC,HHMMSS.sss,A,lat,N,lon,W,speed,course,DDMMYY,...
char copy[256];
strncpy(copy, sentence, sizeof(copy) - 1);
copy[sizeof(copy) - 1] = '\0';
char* token = strtok(copy, ",");
int field = 0;
while (token != NULL && field < 10) {
switch (field) {
case 1: // Time
if (strlen(token) >= 6) {
snprintf(gpsTime, sizeof(gpsTime), "%c%c:%c%c:%s",
token[0], token[1], token[2], token[3], token + 4);
}
break;
case 2: // Fix status
gpsFix = (token[0] == 'A');
break;
case 9: // Date
if (strlen(token) >= 6) {
snprintf(gpsDate, sizeof(gpsDate), "%c%c/%c%c/%c%c",
token[0], token[1], token[2], token[3], token[4], token[5]);
}
break;
}
token = strtok(NULL, ",");
field++;
}
// Update GPS bits of tsStatus
tsStatus = (tsStatus & 0x0C); // Keep MFAM PPS bits (3,2)
tsStatus |= 0x01; // Bit 0: RMC sentence received
if (gpsFix) {
tsStatus |= 0x02; // Bit 1: GPS fix valid
}
}
// ---- MFAM DATA PARSING ----
int16_t toSigned16(uint16_t val) {
if (val > 32767) return (int16_t)(val - 65536);
return (int16_t)val;
}
void parseAuxChannels(uint8_t* sample, uint16_t frameID) {
uint8_t auxID = (frameID >> 11) & 0x07;
uint16_t aux0 = sample[16] | (sample[17] << 8);
uint16_t aux1 = sample[18] | (sample[19] << 8);
uint16_t aux2 = sample[20] | (sample[21] << 8);
uint16_t aux3 = sample[22] | (sample[23] << 8);
switch (auxID) {
case 1:
compassX = toSigned16(aux0) / 0.01333333;
compassY = toSigned16(aux1) / 0.01333333;
compassZ = toSigned16(aux2) / 0.01333333;
compassT = toSigned16(aux3) / 128.0 + 25.0;
break;
case 2:
gyroX = toSigned16(aux0) / 16.384;
gyroY = toSigned16(aux1) / 16.384;
gyroZ = toSigned16(aux2) / 16.384;
gyroT = toSigned16(aux3) / 512.0 + 23.0;
break;
case 4:
accelX = toSigned16(aux0) / 16384.0;
accelY = toSigned16(aux1) / 16384.0;
accelZ = toSigned16(aux2) / 16384.0;
accelT = toSigned16(aux3) / 512.0 + 23.0;
break;
default:
break;
}
}
void writeSample(unsigned long timestamp, uint16_t fiducial, double mag1, uint16_t mag1s,
double mag2, uint16_t mag2s, uint16_t sysStatus) {
char tsStr[12];
snprintf(tsStr, sizeof(tsStr), "%d%d%d%d%d%d%d%d",
(tsStatus >> 7) & 1, (tsStatus >> 6) & 1, (tsStatus >> 5) & 1, (tsStatus >> 4) & 1,
(tsStatus >> 3) & 1, (tsStatus >> 2) & 1, (tsStatus >> 1) & 1, tsStatus & 1);
char sysHex[8], m1sHex[8], m2sHex[8];
snprintf(sysHex, sizeof(sysHex), "%04X", sysStatus);
snprintf(m1sHex, sizeof(m1sHex), "%04X", mag1s);
snprintf(m2sHex, sizeof(m2sHex), "%04X", mag2s);
char line[512];
snprintf(line, sizeof(line),
"%10.4f,%10.4f,%4d,%s,%s,%s,%10.2f,%10.2f,%10.2f,%5.1f,%8.5f,%8.5f,%8.5f,%5.1f,%8.1f,%8.1f,%8.1f,%5.1f,%s,%s,%s,%s",
mag1, mag2, fiducial, sysHex, m1sHex, m2sHex,
gyroX, gyroY, gyroZ, gyroT,
accelX, accelY, accelZ, accelT,
compassX, compassY, compassZ, compassT,
gpsDate, gpsTime, tsStr, gpsString);
if (sdReady && logFile) {
logFile.println(line);
if (sampleCount % 1000 == 0) {
logFile.flush();
}
}
if (sampleCount % 500 == 0) {
Serial.println(line);
}
}
void parsePacket(uint8_t* pkt) {
// Get PPS bits from MFAM system status
uint16_t firstSysStatus = pkt[HEADER_SIZE + 2] | (pkt[HEADER_SIZE + 3] << 8);
uint8_t mfamPPSbits = (firstSysStatus >> 12) & 0x0C;
tsStatus = (tsStatus & 0x03) | mfamPPSbits;
for (int i = 0; i < NUM_SAMPLES; i++) {
int offset = HEADER_SIZE + (i * SAMPLE_SIZE);
uint16_t frameID = pkt[offset] | (pkt[offset + 1] << 8);
uint16_t fiducial = frameID & 0x07FF;
uint16_t sysStatus = pkt[offset + 2] | (pkt[offset + 3] << 8);
uint32_t mag1raw = pkt[offset + 4] | (pkt[offset + 5] << 8) |
((uint32_t)pkt[offset + 6] << 16) | ((uint32_t)pkt[offset + 7] << 24);
double mag1 = mag1raw * 0.05 / 1000.0;
uint16_t mag1status = pkt[offset + 8] | (pkt[offset + 9] << 8);
uint32_t mag2raw = pkt[offset + 10] | (pkt[offset + 11] << 8) |
((uint32_t)pkt[offset + 12] << 16) | ((uint32_t)pkt[offset + 13] << 24);
double mag2 = mag2raw * 0.05 / 1000.0;
uint16_t mag2status = pkt[offset + 14] | (pkt[offset + 15] << 8);
parseAuxChannels(pkt + offset, frameID);
sampleCount++;
downsampleCounter++;
if (downsampleCounter >= DOWNSAMPLE_FACTOR) {
downsampleCounter = 0;
writeSample(millis(), fiducial, mag1, mag1status, mag2, mag2status, sysStatus);
}
}
}
25/03/2026 11:31 am
Nice work! Thanks for sharing!