diff --git a/flix/log.ino b/flix/log.ino index 396bbed..0309ee4 100644 --- a/flix/log.ino +++ b/flix/log.ino @@ -4,10 +4,10 @@ // In-RAM logging #include "vector.h" +#include "util.h" #define LOG_RATE 100 #define LOG_DURATION 10 -#define LOG_PERIOD 1.0 / LOG_RATE #define LOG_SIZE LOG_DURATION * LOG_RATE Vector attitudeEuler; @@ -46,9 +46,8 @@ void prepareLogData() { void logData() { if (!armed) return; static int logPointer = 0; - static float logTime = 0; - if (t - logTime < LOG_PERIOD) return; - logTime = t; + static Rate period(LOG_RATE); + if (!period) return; prepareLogData(); diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 8b30e6a..7d90517 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -6,10 +6,11 @@ #if WIFI_ENABLED #include +#include "util.h" #define SYSTEM_ID 1 -#define PERIOD_SLOW 1.0 -#define PERIOD_FAST 0.1 +#define MAVLINK_RATE_SLOW 1 +#define MAVLINK_RATE_FAST 10 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f bool mavlinkConnected = false; @@ -26,15 +27,12 @@ void processMavlink() { void sendMavlink() { sendMavlinkPrint(); - static float lastSlow = 0; - static float lastFast = 0; - mavlink_message_t msg; uint32_t time = t * 1000; - if (t - lastSlow >= PERIOD_SLOW) { - lastSlow = t; + static Rate slow(MAVLINK_RATE_SLOW), fast(MAVLINK_RATE_FAST); + if (slow) { mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, (armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) | ((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) | @@ -49,9 +47,7 @@ void sendMavlink() { sendMessage(&msg); } - if (t - lastFast >= PERIOD_FAST && mavlinkConnected) { - lastFast = t; - + if (fast && mavlinkConnected) { const float zeroQuat[] = {0, 0, 0, 0}; mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd diff --git a/flix/parameters.ino b/flix/parameters.ino index ef2f6a3..265f966 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -4,6 +4,7 @@ // Parameters storage in flash memory #include +#include "util.h" extern float channelZero[16]; extern float channelMax[16]; @@ -118,10 +119,9 @@ bool setParameter(const char *name, const float value) { } void syncParameters() { - static float lastSync = 0; - if (t - lastSync < 1) return; // sync once per second + static Rate rate(1); + if (!rate) return; // sync once per second if (motorsActive()) return; // don't use flash while flying, it may cause a delay - lastSync = t; for (auto ¶meter : parameters) { if (parameter.value == *parameter.variable) continue; diff --git a/flix/util.h b/flix/util.h index d6ac26d..ee69a43 100644 --- a/flix/util.h +++ b/flix/util.h @@ -54,12 +54,31 @@ void splitString(String& str, String& token0, String& token1, String& token2) { token2 = strtok(NULL, ""); } +// Helper for running code at a fixed rate +class Rate { +public: + float rate; + float last = NAN; + Rate(float rate) : rate(rate) {} + + operator bool() { + if (isnan(last)) { + last = t; + return true; + } + if (t - last >= 1 / rate) { + last = t; + return true; + } + return false; + } +}; + // Delay filter for boolean signals - ensures the signal is on for at least 'delay' seconds class Delay { public: float delay; float start = NAN; - Delay(float delay) : delay(delay) {} bool update(bool on) {