Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Cleanups
Oleg Kalachev82276ddb92ec
diff --git a/flix/cli.ino b/flix/cli.inoindex 4c306b7..ca297b9 100644--- a/flix/cli.ino+++ b/flix/cli.ino@@ -1,7 +1,6 @@// Copyright (c) 2023 Oleg Kalachev <[email protected]>// Repository: https://github.com/okalachev/flix-#include <MPU9250.h>#include "pid.h"static String command;@@ -9,7 +8,6 @@ static String value;static bool parsingCommand = true;extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;-extern MPU9250 IMU;const char* motd ="\nWelcome to\n"@@ -68,7 +66,7 @@ static void doCommand()} else if (command == "psq") {Serial.printf("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);} else if (command == "imu") {- Serial.printf("gyro bias %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads());+ printIMUCal();} else if (command == "rc") {Serial.printf("RAW throttle %d yaw %d pitch %d roll %d aux %d mode %d\n",channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH],diff --git a/flix/flix.ino b/flix/flix.inoindex 94a9e6d..13bd19f 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -21,7 +21,6 @@#define MOTOR_FRONT_RIGHT 2#define MOTOR_REAR_RIGHT 1-uint32_t startTime; // system startup timeuint32_t stepTime; // current step timeuint32_t steps; // total steps countfloat stepsPerSecond; // steps per last second@@ -33,7 +32,6 @@ float motors[4]; // normalized motors thrust in range [-1..1]Vector rates; // angular rates, rad/sVector acc; // accelerometer data, m/s/sQuaternion attitude; // estimated attitude-bool calibrating; // flag we're calibratingvoid setup(){diff --git a/flix/imu.ino b/flix/imu.inoindex 9c74121..763fe8e 100644--- a/flix/imu.ino+++ b/flix/imu.ino@@ -55,17 +55,12 @@ bool readIMU()static void calibrateGyro(){- calibrating = true;Serial.println("Calibrating gyro, stand still");delay(500);int status = IMU.calibrateGyro();Serial.printf("Calibration status: %d\n", status);- Serial.print("Gyro bias: ");- Serial.print(IMU.getGyroBiasX_rads(), 10); Serial.print(" ");- Serial.print(IMU.getGyroBiasY_rads(), 10); Serial.print(" ");- Serial.println(IMU.getGyroBiasZ_rads(), 10);IMU.setSrd(0);- calibrating = false;+ printIMUCal();}static void calibrateAccel()@@ -82,14 +77,7 @@ static void calibrateAccel()IMU.calibrateAccel();Serial.println("Cal accel: upside down"); delay(300);IMU.calibrateAccel();- Serial.print("Accel bias: ");- Serial.print(IMU.getAccelBiasX_mss(), 10); Serial.print(" ");- Serial.print(IMU.getAccelBiasY_mss(), 10); Serial.print(" ");- Serial.println(IMU.getAccelBiasZ_mss(), 10);- Serial.print("Accel scale: ");- Serial.print(IMU.getAccelScaleFactorX(), 10); Serial.print(" ");- Serial.print(IMU.getAccelScaleFactorY(), 10); Serial.print(" ");- Serial.println(IMU.getAccelScaleFactorZ(), 10);+ printIMUCal();}static void loadAccelCal()@@ -106,6 +94,13 @@ static void loadGyroCal()IMU.setGyroBiasZ_rads(0.0163032326);}+void printIMUCal()+{+ Serial.printf("gyro bias: %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads());+ Serial.printf("accel bias: %f %f %f\n", IMU.getAccelBiasX_mss(), IMU.getAccelBiasY_mss(), IMU.getAccelBiasZ_mss());+ Serial.printf("accel scale: %f %f %f\n", IMU.getAccelScaleFactorX(), IMU.getAccelScaleFactorY(), IMU.getAccelScaleFactorZ());+}+// Accel bias: 0.0463809967 0.0463809967 0.1486964226// Accel scale: 0.9986976385 0.9993721247 1.0561490059diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex df7191a..d4c74d0 100644--- a/flix/mavlink.ino+++ b/flix/mavlink.ino@@ -21,7 +21,7 @@ void sendMavlink()mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR,MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED,- 0, calibrating ? MAV_STATE_CALIBRATING : MAV_STATE_STANDBY);+ 0, MAV_STATE_STANDBY);sendMessage(&msg);// params testdiff --git a/gazebo/MPU9250.h b/gazebo/MPU9250.hdeleted file mode 100644index 9f118d9..0000000--- a/gazebo/MPU9250.h+++ /dev/null@@ -1,17 +0,0 @@-// Copyright (c) 2023 Oleg Kalachev <[email protected]>-// Repository: https://github.com/okalachev/flix-// Distributed under the MIT License (https://opensource.org/licenses/MIT)--// Mocks of some MPU9250 library functions--#pragma once--class MPU9250 {-public:- float getAccelBiasX_mss() { return 0; }- float getAccelBiasY_mss() { return 0; }- float getAccelBiasZ_mss() { return 0; }- float getGyroBiasX_rads() { return 0; }- float getGyroBiasY_rads() { return 0; }- float getGyroBiasZ_rads() { return 0; }-};diff --git a/gazebo/flix.cpp b/gazebo/flix.cppindex c80bd07..0fb6177 100644--- a/gazebo/flix.cpp+++ b/gazebo/flix.cpp@@ -140,11 +140,6 @@ public:const double maxThrust = 0.03 * ONE_G; // 30 g, https://www.youtube.com/watch?v=VtKI4Pjx8Sk// 65 mm prop ~40 g- // std::cout << "fr: " << motors[MOTOR_FRONT_RIGHT]- // << " fl: " << motors[MOTOR_FRONT_LEFT]- // << " rr: " << motors[MOTOR_REAR_RIGHT]- // << " rl: " << motors[MOTOR_REAR_LEFT] << std::endl;-const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05;const float minThrustRel = 0;