Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Make dt=0 on first step, simplify code

Oleg Kalachev9a93367629b1
diff --git a/flix/estimate.ino b/flix/estimate.ino
index ecc8990..713d538 100644
--- a/flix/estimate.ino
+++ b/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()
diff --git a/flix/flix.ino b/flix/flix.ino
index 4002226..65d534b 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -62,5 +62,4 @@ void loop()
sendMavlink();
#endif
logData();
- signalizeHorizontality();
}
diff --git a/flix/pid.h b/flix/pid.h
index 1b2de4d..491e11b 100644
--- a/flix/pid.h
+++ b/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;
diff --git a/flix/time.ino b/flix/time.ino
index c2e93c8..00e2051 100644
--- a/flix/time.ino
+++ b/flix/time.ino
@@ -1,14 +1,16 @@
// Copyright (c) 2023 Oleg Kalachev <[email protected]>
// 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;
diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h
index a18bbbf..3ef4409 100644
--- a/gazebo/Arduino.h
+++ b/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
diff --git a/gazebo/flix.h b/gazebo/flix.h
index dce7089..1e80fae 100644
--- a/gazebo/flix.h
+++ b/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();