mirror of
https://github.com/okalachev/flix.git
synced 2025-07-28 20:08:53 +00:00
6b7601c0bd
Make the order or basic methods consistent between Vector and Quaternion. Remove `ZYX` from Euler method names as this is standard for robotics. Rename angular rates to rotation vector, which is more correct. Make rotation methods static, to keep the arguments order consistent. Make `Quaternion::fromAxisAngle` accept Vector for axis. Minor fixes.
80 lines
1.8 KiB
C++
80 lines
1.8 KiB
C++
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// In-RAM logging
|
|
|
|
#include "vector.h"
|
|
|
|
#define LOG_RATE 100
|
|
#define LOG_DURATION 10
|
|
#define LOG_PERIOD 1.0 / LOG_RATE
|
|
#define LOG_SIZE LOG_DURATION * LOG_RATE
|
|
|
|
float tFloat;
|
|
Vector attitudeEuler;
|
|
Vector attitudeTargetEuler;
|
|
|
|
struct LogEntry {
|
|
const char *name;
|
|
float *value;
|
|
};
|
|
|
|
LogEntry logEntries[] = {
|
|
{"t", &tFloat},
|
|
{"rates.x", &rates.x},
|
|
{"rates.y", &rates.y},
|
|
{"rates.z", &rates.z},
|
|
{"ratesTarget.x", &ratesTarget.x},
|
|
{"ratesTarget.y", &ratesTarget.y},
|
|
{"ratesTarget.z", &ratesTarget.z},
|
|
{"attitude.x", &attitudeEuler.x},
|
|
{"attitude.y", &attitudeEuler.y},
|
|
{"attitude.z", &attitudeEuler.z},
|
|
{"attitudeTarget.x", &attitudeTargetEuler.x},
|
|
{"attitudeTarget.y", &attitudeTargetEuler.y},
|
|
{"attitudeTarget.z", &attitudeTargetEuler.z},
|
|
{"thrustTarget", &thrustTarget}
|
|
};
|
|
|
|
const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]);
|
|
float logBuffer[LOG_SIZE][logColumns];
|
|
|
|
void prepareLogData() {
|
|
tFloat = t;
|
|
attitudeEuler = attitude.toEuler();
|
|
attitudeTargetEuler = attitudeTarget.toEuler();
|
|
}
|
|
|
|
void logData() {
|
|
if (!armed) return;
|
|
static int logPointer = 0;
|
|
static double logTime = 0;
|
|
if (t - logTime < LOG_PERIOD) return;
|
|
logTime = t;
|
|
|
|
prepareLogData();
|
|
|
|
for (int i = 0; i < logColumns; i++) {
|
|
logBuffer[logPointer][i] = *logEntries[i].value;
|
|
}
|
|
|
|
logPointer++;
|
|
if (logPointer >= LOG_SIZE) {
|
|
logPointer = 0;
|
|
}
|
|
}
|
|
|
|
void dumpLog() {
|
|
// Print header
|
|
for (int i = 0; i < logColumns; i++) {
|
|
print("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n");
|
|
}
|
|
// Print data
|
|
for (int i = 0; i < LOG_SIZE; i++) {
|
|
if (logBuffer[i][0] == 0) continue; // skip empty records
|
|
for (int j = 0; j < logColumns; j++) {
|
|
print("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n");
|
|
}
|
|
}
|
|
}
|