Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Add Rate class for running the code at fixed rate
Oleg Kalachev0e6651ab8258
diff --git a/flix/log.ino b/flix/log.inoindex 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_RATEVector 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.inoindex 8b30e6a..7d90517 100644--- a/flix/mavlink.ino+++ b/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 PERIOD_FAST 0.1+#define MAVLINK_RATE_SLOW 1+#define MAVLINK_RATE_FAST 10#define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1fbool 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 frddiff --git a/flix/parameters.ino b/flix/parameters.inoindex ef2f6a3..265f966 100644--- a/flix/parameters.ino+++ b/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;- if (t - lastSync < 1) return; // sync once per second+ static Rate rate(1);+ if (!rate) return; // sync once per secondif (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.hindex 21c416a..025cd5e 100644--- a/flix/util.h+++ b/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' secondsclass Delay {public:float delay;float start = NAN;-Delay(float delay) : delay(delay) {}bool update(bool on) {if (!on) {start = NAN;return false;- }- if (isnan(start)) {+ } else if (isnan(start)) {start = t;}return t - start >= delay;