Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Use FLU as the main coordinate system instead of FRD Corresponding to the IMU orientation in the new version
Oleg Kalachevabcc9b96dea0
diff --git a/flix/control.ino b/flix/control.inoindex ef6eb8a..f6c4a03 100644--- a/flix/control.ino+++ b/flix/control.ino@@ -81,22 +81,22 @@ void interpretRC() {if (mode == ACRO) {yawMode = YAW_RATE;ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;- ratesTarget.y = -controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; // up pitch stick means tilt clockwise in frd- ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;+ ratesTarget.y = controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX;+ ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU} else if (mode == STAB) {yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;attitudeTarget = Quaternion::fromEulerZYX(Vector(controls[RC_CHANNEL_ROLL] * MAX_TILT,- -controls[RC_CHANNEL_PITCH] * MAX_TILT,+ controls[RC_CHANNEL_PITCH] * MAX_TILT,attitudeTarget.getYaw()));- ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;+ ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU} else if (mode == MANUAL) {// passthrough modeyawMode = YAW_RATE;- torqueTarget = Vector(controls[RC_CHANNEL_ROLL], -controls[RC_CHANNEL_PITCH], controls[RC_CHANNEL_YAW]) * 0.01;+ torqueTarget = Vector(controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_PITCH], -controls[RC_CHANNEL_YAW]) * 0.01;}if (yawMode == YAW_RATE || !motorsActive()) {@@ -113,7 +113,7 @@ void controlAttitude() {return;}- const Vector up(0, 0, -1);+ const Vector up(0, 0, 1);Vector upActual = attitude.rotate(up);Vector upTarget = attitudeTarget.rotate(up);@@ -150,10 +150,10 @@ void controlTorque() {return;}- motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;- motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;- motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;- motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;+ motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;+ motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;+ motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;+ motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;motors[0] = constrain(motors[0], 0, 1);motors[1] = constrain(motors[1], 0, 1);diff --git a/flix/estimate.ino b/flix/estimate.inoindex 6824d32..ed6506a 100644--- a/flix/estimate.ino+++ b/flix/estimate.ino@@ -36,7 +36,7 @@ void applyAcc() {if (!landed) return;// calculate accelerometer correction- Vector up = attitude.rotate(Vector(0, 0, -1));+ Vector up = attitude.rotate(Vector(0, 0, 1));Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC;// apply correction@@ -45,6 +45,6 @@ void applyAcc() {}void signalizeHorizontality() {- float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));+ float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, 1)), Vector(0, 0, 1));setLED(angle < radians(15));}diff --git a/flix/imu.ino b/flix/imu.inoindex 45660e8..e58460e 100644--- a/flix/imu.ino+++ b/flix/imu.ino@@ -2,6 +2,9 @@// Repository: https://github.com/okalachev/flix// Work with the IMU sensor+// IMU is oriented FLU (front-left-up) style.+// In case of FRD (front-right-down) orientation of the IMU, use this code:+// https://gist.github.com/okalachev/713db47e31bce643dbbc9539d166ce98.#include <SPI.h>#include <MPU9250.h>diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex d4c6480..de7602a 100644--- a/flix/mavlink.ino+++ b/flix/mavlink.ino@@ -38,8 +38,9 @@ void sendMavlink() {lastFast = t;const float zeroQuat[] = {0, 0, 0, 0};+ Quaternion attitudeFRD = FLU2FRD(attitude); // MAVLink uses FRD coordinate systemmavlink_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);+ time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat);sendMessage(&msg);mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0,@@ -97,4 +98,9 @@ void handleMavlink(const void *_msg) {}}+// Convert Forward-Left-Up to Forward-Right-Down quaternion+inline Quaternion FLU2FRD(const Quaternion &q) {+ return Quaternion(q.w, q.x, -q.y, -q.z);+}+#endifdiff --git a/gazebo/flix.h b/gazebo/flix.hindex 7debf3e..8b261bd 100644--- a/gazebo/flix.h+++ b/gazebo/flix.h@@ -49,6 +49,7 @@ void sendMavlink();void sendMessage(const void *msg);void receiveMavlink();void handleMavlink(const void *_msg);+inline Quaternion FLU2FRD(const Quaternion &q);// mocksvoid setLED(bool on) {};diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cppindex f36ec9b..d789bf0 100644--- a/gazebo/simulator.cpp+++ b/gazebo/simulator.cpp@@ -17,7 +17,6 @@#include "Arduino.h"#include "flix.h"-#include "util.h"#include "util.ino"#include "rc.ino"#include "time.ino"@@ -63,9 +62,9 @@ public:__micros = model->GetWorld()->SimTime().Double() * 1000000;step();- // read imu- gyro = flu2frd(imu->AngularVelocity());- acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));+ // read virtual imu+ gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());+ acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));// read rcreadRC();@@ -75,7 +74,7 @@ public:estimate();// correct yaw to the actual yaw- attitude.setYaw(-this->model->WorldPose().Yaw());+ attitude.setYaw(this->model->WorldPose().Yaw());control();parseInput();diff --git a/gazebo/util.h b/gazebo/util.hdeleted file mode 100644index 7467a2c..0000000--- a/gazebo/util.h+++ /dev/null@@ -1,14 +0,0 @@-#include <ignition/math/Vector3.hh>-#include <ignition/math/Pose3.hh>--using ignition::math::Vector3d;-using ignition::math::Pose3d;--Pose3d flu2frd(const Pose3d& p) {- return ignition::math::Pose3d(p.Pos().X(), -p.Pos().Y(), -p.Pos().Z(),- p.Rot().W(), p.Rot().X(), -p.Rot().Y(), -p.Rot().Z());-}--Vector flu2frd(const Vector3d& v) {- return Vector(v.X(), -v.Y(), -v.Z());-}