Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Use double for storing time instead of float Float precision may be not enough after some time of operating
Oleg Kalachev70f5186c1be0
diff --git a/docs/firmware.md b/docs/firmware.mdindex 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/s<sup>2</sup>*.diff --git a/flix/cli.ino b/flix/cli.inoindex 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.inoindex 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.inoindex 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, sfloat dt; // time delta from previous step, sint16_t channels[16]; // raw rc channelsfloat controls[16]; // normalized controls in range [-1..1] ([0..1] for throttle)diff --git a/flix/log.ino b/flix/log.inoindex 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.inoindex 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.inoindex 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 secondif (motorsActive()) return; // don't use flash while flying, it may cause a delaylastSync = t;diff --git a/flix/rc.ino b/flix/rc.inoindex 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 updatefloat channelNeutral[16] = {NAN}; // first element NAN means not calibratedfloat channelMax[16];diff --git a/flix/time.ino b/flix/time.inoindex b57b5bc..3c6cb0f 100644--- a/flix/time.ino+++ b/flix/time.ino@@ -6,7 +6,7 @@float loopRate; // Hzvoid 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 windowdiff --git a/gazebo/flix.h b/gazebo/flix.hindex 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