Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Make dt=0 on first step, simplify code
Oleg Kalachev9a93367629b1
diff --git a/flix/estimate.ino b/flix/estimate.inoindex 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 gyroattitude *= Quaternion::fromAngularRates(rates * dt);attitude.normalize();+}- // test should we apply acc+void applyAcc()+{+ // test should we apply accelerometer gravity correctionfloat 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 acceleratingreturn;}+ // calculate accelerometer correctionVector 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 correctionattitude *= 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.inoindex 4002226..65d534b 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -62,5 +62,4 @@ void loop()sendMavlink();#endiflogData();- signalizeHorizontality();}diff --git a/flix/pid.h b/flix/pid.hindex 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.inoindex 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 windowloopFreq = freq;windowStart = t;freq = 0;diff --git a/gazebo/Arduino.h b/gazebo/Arduino.hindex 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.295779513082320876798154814105diff --git a/gazebo/flix.h b/gazebo/flix.hindex 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;// declarationsvoid computeLoopFreq();+void applyGyro();+void applyAcc();+void signalizeHorizontality();void control();void interpretRC();static void controlAttitude();