Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Store current time in float variable
Oleg Kalachevc1aa0d9869c7
diff --git a/flix/cli.ino b/flix/cli.inoindex 513ad2e..510ccf2 100644--- a/flix/cli.ino+++ b/flix/cli.ino@@ -53,7 +53,7 @@ static const struct Param {{"yi", &yawRatePID.i, nullptr},{"yd", &yawRatePID.d, nullptr},- {"ss", &stepsPerSecond, nullptr},+ {"ss", &loopFreq, nullptr},// {"m", &mode, nullptr},};diff --git a/flix/flix.ino b/flix/flix.inoindex 13bd19f..9828941 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -21,10 +21,9 @@#define MOTOR_FRONT_RIGHT 2#define MOTOR_REAR_RIGHT 1-uint32_t stepTime; // current step time-uint32_t steps; // total steps count-float stepsPerSecond; // steps per last second-float dt; // time delta from previous step+float t = NAN; // current step time, s+float dt; // time delta from previous step, s+float loopFreq; // loop frequency, Hzuint16_t channels[16]; // raw rc channelsfloat controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)uint32_t rcFailSafe, rcLostFrame;diff --git a/flix/log.ino b/flix/log.inoindex 90b1c7e..834ca00 100644--- a/flix/log.ino+++ b/flix/log.ino@@ -5,21 +5,22 @@const int LOG_RATE = 100;const int LOG_DURATION = 10;-const int LOG_PERIOD = 1000000 / LOG_RATE;+const float LOG_PERIOD = 1 / LOG_RATE;const int LOG_SIZE = LOG_DURATION * LOG_RATE;const int LOG_COLUMNS = 14;static float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)static int logPointer = 0;-static uint32_t lastLog = 0;void logData(){if (!armed) return;- if (stepTime - lastLog < LOG_PERIOD) return;- lastLog = stepTime;- logBuffer[logPointer][0] = stepTime;+ static float logTime = 0;+ if (t - logTime < LOG_PERIOD) return;+ logTime = t;++ logBuffer[logPointer][0] = t;logBuffer[logPointer][1] = rates.x;logBuffer[logPointer][2] = rates.y;logBuffer[logPointer][3] = rates.z;diff --git a/flix/time.ino b/flix/time.inoindex b15a8fd..c2e93c8 100644--- a/flix/time.ino+++ b/flix/time.ino@@ -1,28 +1,25 @@// Copyright (c) 2023 Oleg Kalachev <[email protected]>// Repository: https://github.com/okalachev/flix-const uint32_t MS = 1000;-const uint32_t S = 1000000;+#define FREQ_WINDOW 1-static uint32_t stepsPerSecondCurrent;-static uint32_t stepsPerSecondCurrentLast;+void step()+{+ float now = micros() / 1000000.0;+ dt = now - t; // dt is NAN on first step+ t = now;-void step() {- steps++;- auto time = micros();-- if (stepTime == 0) { // first step- stepTime = time;- }-- dt = (time - stepTime) / 1000000.0;- stepTime = time;+ computeLoopFreq();+}- // compute steps per seconds- stepsPerSecondCurrent++;- if (time - stepsPerSecondCurrentLast >= 1000000) {- stepsPerSecond = stepsPerSecondCurrent;- stepsPerSecondCurrent = 0;- stepsPerSecondCurrentLast = time;+void computeLoopFreq()+{+ static float windowStart = 0;+ static uint32_t freq = 0;+ freq++;+ if (t - windowStart >= FREQ_WINDOW) {+ loopFreq = freq;+ windowStart = t;+ freq = 0;}}diff --git a/gazebo/flix.h b/gazebo/flix.hindex 83d59b5..496c233 100644--- a/gazebo/flix.h+++ b/gazebo/flix.h@@ -15,11 +15,9 @@#define MOTOR_FRONT_RIGHT 2#define MOTOR_REAR_RIGHT 1-uint32_t startTime;-uint32_t stepTime;-uint32_t steps;-float stepsPerSecond;+float t;float dt;+float loopFreq;float motors[4];int16_t channels[16]; // raw rc channels WARNING: unsigned on hardwarefloat controls[RC_CHANNELS];@@ -27,7 +25,8 @@ Vector acc;Vector rates;Quaternion attitude;-// control+// declarations+void computeLoopFreq();void control();void interpretRC();static void controlAttitude();