From 70f5186c1be0a11cfbe6a14b7e3145a67ef90a72 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 12 Jan 2025 19:58:36 +0300 Subject: [PATCH] Use double for storing time instead of float Float precision may be not enough after some time of operating --- docs/firmware.md | 2 +- flix/cli.ino | 2 +- flix/failsafe.ino | 2 +- flix/flix.ino | 2 +- flix/log.ino | 6 ++++-- flix/mavlink.ino | 6 +++--- flix/parameters.ino | 2 +- flix/rc.ino | 2 +- flix/time.ino | 4 ++-- gazebo/flix.h | 2 +- 10 files changed, 16 insertions(+), 14 deletions(-) diff --git a/docs/firmware.md b/docs/firmware.md index b073d68..3b9b45a 100644 --- a/docs/firmware.md +++ b/docs/firmware.md @@ -6,7 +6,7 @@ The main loop is running at 1000 Hz. All the dataflow is happening through global variables (for simplicity): -* `t` *(float)* — current step time, *s*. +* `t` *(double)* — current step time, *s*. * `dt` *(float)* — time delta between the current and previous steps, *s*. * `gyro` *(Vector)* — data from the gyroscope, *rad/s*. * `acc` *(Vector)* — acceleration data from the accelerometer, *m/s2*. diff --git a/flix/cli.ino b/flix/cli.ino index 530ca05..0165b02 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -8,7 +8,7 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; extern float loopRate, dt; -extern float t; +extern double t; extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel; const char* motd = diff --git a/flix/failsafe.ino b/flix/failsafe.ino index d554941..bb572dc 100644 --- a/flix/failsafe.ino +++ b/flix/failsafe.ino @@ -6,7 +6,7 @@ #define RC_LOSS_TIMEOUT 0.2 #define DESCEND_TIME 3.0 // time to descend from full throttle to zero -extern float controlsTime; +extern double controlsTime; extern int rollChannel, pitchChannel, throttleChannel, yawChannel; void failsafe() { diff --git a/flix/flix.ino b/flix/flix.ino index 7e55c43..5671d60 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -10,7 +10,7 @@ #define SERIAL_BAUDRATE 115200 #define WIFI_ENABLED 1 -float t = NAN; // current step time, s +double t = NAN; // current step time, s float dt; // time delta from previous step, s int16_t channels[16]; // raw rc channels float controls[16]; // normalized controls in range [-1..1] ([0..1] for throttle) diff --git a/flix/log.ino b/flix/log.ino index 83aba7f..d6be3e9 100644 --- a/flix/log.ino +++ b/flix/log.ino @@ -10,6 +10,7 @@ #define LOG_PERIOD 1.0 / LOG_RATE #define LOG_SIZE LOG_DURATION * LOG_RATE +float tFloat; Vector attitudeEuler; Vector attitudeTargetEuler; @@ -19,7 +20,7 @@ struct LogEntry { }; LogEntry logEntries[] = { - {"t", &t}, + {"t", &tFloat}, {"rates.x", &rates.x}, {"rates.y", &rates.y}, {"rates.z", &rates.z}, @@ -39,6 +40,7 @@ const int logColumns = sizeof(logEntries) / sizeof(logEntries[0]); float logBuffer[LOG_SIZE][logColumns]; void prepareLogData() { + tFloat = t; attitudeEuler = attitude.toEulerZYX(); attitudeTargetEuler = attitudeTarget.toEulerZYX(); } @@ -46,7 +48,7 @@ void prepareLogData() { void logData() { if (!armed) return; static int logPointer = 0; - static float logTime = 0; + static double logTime = 0; if (t - logTime < LOG_PERIOD) return; logTime = t; diff --git a/flix/mavlink.ino b/flix/mavlink.ino index d09f0e8..68edc4b 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -13,7 +13,7 @@ #define MAVLINK_CONTROL_SCALE 0.7f #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f -extern float controlsTime; +extern double controlsTime; extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel; void processMavlink() { @@ -22,8 +22,8 @@ void processMavlink() { } void sendMavlink() { - static float lastSlow = 0; - static float lastFast = 0; + static double lastSlow = 0; + static double lastFast = 0; mavlink_message_t msg; uint32_t time = t * 1000; diff --git a/flix/parameters.ino b/flix/parameters.ino index 3e9a6ff..ee8526d 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -113,7 +113,7 @@ bool setParameter(const char *name, const float value) { } void syncParameters() { - static float lastSync = 0; + static double lastSync = 0; if (t - lastSync < 1) return; // sync once per second if (motorsActive()) return; // don't use flash while flying, it may cause a delay lastSync = t; diff --git a/flix/rc.ino b/flix/rc.ino index 91dec8a..0899b82 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -14,7 +14,7 @@ int yawChannel = 3; int armedChannel = 4; int modeChannel = 5; -float controlsTime; // time of the last controls update +double controlsTime; // time of the last controls update float channelNeutral[16] = {NAN}; // first element NAN means not calibrated float channelMax[16]; diff --git a/flix/time.ino b/flix/time.ino index b57b5bc..3c6cb0f 100644 --- a/flix/time.ino +++ b/flix/time.ino @@ -6,7 +6,7 @@ float loopRate; // Hz void step() { - float now = micros() / 1000000.0; + double now = micros() / 1000000.0; dt = now - t; t = now; @@ -18,7 +18,7 @@ void step() { } void computeLoopRate() { - static float windowStart = 0; + static double windowStart = 0; static uint32_t rate = 0; rate++; if (t - windowStart >= 1) { // 1 second window diff --git a/gazebo/flix.h b/gazebo/flix.h index 09f9cfa..b460170 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -12,7 +12,7 @@ #define WIFI_ENABLED 1 -float t = NAN; +double t = NAN; float dt; float motors[4]; int16_t channels[16]; // raw rc channels