Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Store current time in float variable

Oleg Kalachevc1aa0d9869c7
diff --git a/flix/cli.ino b/flix/cli.ino
index 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.ino
index 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, Hz
uint16_t channels[16]; // raw rc channels
float 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.ino
index 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.ino
index 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.h
index 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 hardware
float controls[RC_CHANNELS];
@@ -27,7 +25,8 @@ Vector acc;
Vector rates;
Quaternion attitude;
-// control
+// declarations
+void computeLoopFreq();
void control();
void interpretRC();
static void controlAttitude();