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 // 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(); + computeLoopFreq(); +} - if (stepTime == 0) { // first step - stepTime = time; - } - - dt = (time - stepTime) / 1000000.0; - stepTime = time; - - // 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();