Add Rate class for running the code at fixed rate

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;
+	static Rate period(LOG_RATE);
-	if (t - logTime < LOG_PERIOD) return;
+	if (!period) return;
-	logTime = t;
 
 	prepareLogData();
 

flix/mavlink.ino

@@ -6,10 +6,11 @@
 #if WIFI_ENABLED
 
 #include <MAVLink.h>
+#include "util.h"
 
 #define SYSTEM_ID 1
-#define PERIOD_SLOW 1.0
+#define MAVLINK_RATE_SLOW 1
-#define PERIOD_FAST 0.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) {
+	static Rate slow(MAVLINK_RATE_SLOW), fast(MAVLINK_RATE_FAST);
-		lastSlow = t;
 
+	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) {
+	if (fast && mavlinkConnected) {
-		lastFast = t;
-
 		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

flix/parameters.ino

@@ -4,6 +4,7 @@
 // Parameters storage in flash memory
 
 #include <Preferences.h>
+#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;
+	static Rate rate(1);
-	if (t - lastSync < 1) return; // sync once per second
+	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 &parameter : parameters) {
 		if (parameter.value == *parameter.variable) continue;

flix/util.h

@@ -50,20 +50,34 @@ void splitString(String& str, String& token0, String& token1, String& token2) {
 	token2 = strtok(NULL, "");
 }
 
+// Rate limiter
+class Rate {
+public:
+	float rate;
+	float last = 0;
+	Rate(float rate) : rate(rate) {}
+
+	operator bool() {
+		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) {
 		if (!on) {
 			start = NAN;
 			return false;
-		}
+		} else if (isnan(start)) {
-		if (isnan(start)) {
 			start = t;
 		}
 		return t - start >= delay;