Make dt=0 on first step, simplify code

flix/estimate.ino

@@ -13,50 +13,36 @@
 
 void estimate()
 {
-	if (dt == 0) {
-		return;
-	}
+	applyGyro();
+	applyAcc();
+	signalizeHorizontality();
+}
 
+void applyGyro()
+{
 	// applying gyro
 	attitude *= Quaternion::fromAngularRates(rates * dt);
 	attitude.normalize();
+}
 
-	// test should we apply acc
+void applyAcc()
+{
+	// test should we apply accelerometer gravity correction
 	float accNorm = acc.norm();
 	if (accNorm < ACC_MIN * ONE_G || accNorm > ACC_MAX * ONE_G) {
-		// use acc only when we're not accelerating
+		// use accelerometer only when we're not accelerating
 		return;
 	}
 
+	// calculate accelerometer correction
 	Vector up = attitude.rotate(Vector(0, 0, -1));
 	Vector accCorrDirection = Vector::angularRatesBetweenVectors(acc, up);
 	accCorrDirection.normalize();
-
-	if (!accCorrDirection.finite()) {
-		return;
-	}
-
 	Vector accCorr = accCorrDirection * Vector::angleBetweenVectors(up, acc) * dt * WEIGHT_ACC;
 
-	if (!accCorr.finite()) {
-		return; // TODO
-	}
-
+	// apply correction
 	attitude *= Quaternion::fromAngularRates(accCorr);
 	attitude.normalize();
-
-	if (!attitude.finite()) {
-		Serial.print("dt "); Serial.println(dt, 15);
-		Serial.print("up "); Serial.println(up);
-		Serial.print("acc "); Serial.println(acc);
-		Serial.print("acc norm "); Serial.println(acc.norm());
-		Serial.print("upp norm "); Serial.println(up.norm());
-		Serial.print("acc dot up "); Serial.println(Vector::dot(up, acc), 15);
-		Serial.print("acc cor ang "); Serial.println(Vector::angleBetweenVectors(up, acc), 15);
-		Serial.print("acc corr dir "); Serial.println(accCorrDirection);
-		Serial.print("acc cor "); Serial.println(accCorr);
-		Serial.print("att "); Serial.println(attitude);
-	}
 }
 
 void signalizeHorizontality()

flix/flix.ino

@@ -62,5 +62,4 @@ void loop()
 	sendMavlink();
 #endif
 	logData();
-	signalizeHorizontality();
 }

flix/pid.h

@@ -20,20 +20,11 @@ public:
 
 	float update(float error, float dt)
 	{
-		if (!isfinite(error) || !isfinite(dt)) {
-			// TODO: brutal way to remove glitches
-			Serial.println("nan in error or dt");
-			return NAN;
-		}
-
-		if (dt > 0) {
-			// calculate integral if dt is valid
-			integral += error * dt;
-			if (isfinite(prevError)) {
-				// calculate derivative if both dt and prevError are valid
-				float _derivative = (error - prevError) / dt;
-				derivative = derivative * 0.8 + 0.2 * _derivative; // lpf WARNING:
-			}
+		integral += error * dt;
+		if (isfinite(prevError) && dt > 0) {
+			// calculate derivative if both dt and prevError are valid
+			float _derivative = (error - prevError) / dt;
+			derivative = derivative * 0.8 + 0.2 * _derivative; // lpf WARNING:
 		}
 
 		prevError = error;

flix/time.ino

@@ -1,14 +1,16 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-#define FREQ_WINDOW 1
-
 void step()
 {
 	float now = micros() / 1000000.0;
-	dt = now - t; // dt is NAN on first step
+	dt = now - t;
 	t = now;
 
+	if (isnan(dt)) {
+		dt = 0; // assume dt to be zero on first step
+	}
+
 	computeLoopFreq();
 }
 
@@ -17,7 +19,7 @@ void computeLoopFreq()
 	static float windowStart = 0;
 	static uint32_t freq = 0;
 	freq++;
-	if (t - windowStart >= FREQ_WINDOW) {
+	if (t - windowStart >= 1) { // 1 second window
 		loopFreq = freq;
 		windowStart = t;
 		freq = 0;

gazebo/Arduino.h

@@ -13,10 +13,6 @@
 #include <unistd.h>
 #include <sys/poll.h>
 
-using std::max;
-using std::min;
-using std::isfinite;
-
 #define PI 3.1415926535897932384626433832795
 #define DEG_TO_RAD 0.017453292519943295769236907684886
 #define RAD_TO_DEG 57.295779513082320876798154814105

gazebo/flix.h

@@ -7,6 +7,7 @@
 #include <stdio.h>
 #include "vector.h"
 #include "quaternion.h"
+#include "Arduino.h"
 
 #define RC_CHANNELS 6
 
@@ -15,7 +16,7 @@
 #define MOTOR_FRONT_RIGHT 2
 #define MOTOR_REAR_RIGHT 1
 
-float t;
+float t = NAN;
 float dt;
 float loopFreq;
 float motors[4];
@@ -27,6 +28,9 @@ Quaternion attitude;
 
 // declarations
 void computeLoopFreq();
+void applyGyro();
+void applyAcc();
+void signalizeHorizontality();
 void control();
 void interpretRC();
 static void controlAttitude();