Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Change C++ code style: put curly brace on the same line
Oleg Kalachevd2296fea76b4
diff --git a/flix/cli.ino b/flix/cli.inoindex 9615ee6..2fe3465 100644--- a/flix/cli.ino+++ b/flix/cli.ino@@ -57,8 +57,7 @@ const struct Param {{"t", &t, nullptr},};-void doCommand(String& command, String& value)-{+void doCommand(String& command, String& value) {if (command == "help" || command == "motd") {Serial.println(motd);} else if (command == "show") {@@ -119,8 +118,7 @@ void doCommand(String& command, String& value)}}-void showTable()-{+void showTable() {for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {Serial.print(params[i].name);Serial.print(" ");@@ -128,8 +126,7 @@ void showTable()}}-void cliTestMotor(uint8_t n)-{+void cliTestMotor(uint8_t n) {Serial.printf("Testing motor %d\n", n);motors[n] = 1;sendMotors();@@ -139,8 +136,7 @@ void cliTestMotor(uint8_t n)Serial.println("Done");}-void parseInput()-{+void parseInput() {static bool showMotd = true;static String command;static String value;diff --git a/flix/control.ino b/flix/control.inoindex df7c2bc..d563f36 100644--- a/flix/control.ino+++ b/flix/control.ino@@ -53,8 +53,7 @@ Vector ratesTarget;Vector torqueTarget;float thrustTarget;-void control()-{+void control() {interpretRC();if (mode == STAB) {controlAttitude();@@ -68,8 +67,7 @@ void control()}}-void interpretRC()-{+void interpretRC() {if (controls[RC_CHANNEL_MODE] < 0.25) {mode = MANUAL;} else if (controls[RC_CHANNEL_MODE] < 0.75) {@@ -108,8 +106,7 @@ void interpretRC()}}-void controlAttitude()-{+void controlAttitude() {if (!armed) {rollPID.reset();pitchPID.reset();@@ -131,8 +128,7 @@ void controlAttitude()}}-void controlRate()-{+void controlRate() {if (!armed) {rollRatePID.reset();pitchRatePID.reset();@@ -147,8 +143,7 @@ void controlRate()torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, dt);}-void controlTorque()-{+void controlTorque() {if (!armed) {memset(motors, 0, sizeof(motors));return;@@ -165,13 +160,11 @@ void controlTorque()motors[3] = constrain(motors[3], 0, 1);}-bool motorsActive()-{+bool motorsActive() {return motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;}-const char* getModeName()-{+const char* getModeName() {switch (mode) {case MANUAL: return "MANUAL";case ACRO: return "ACRO";diff --git a/flix/estimate.ino b/flix/estimate.inoindex 6919dcf..a0cc385 100644--- a/flix/estimate.ino+++ b/flix/estimate.ino@@ -9,22 +9,19 @@#define ONE_G 9.807f#define WEIGHT_ACC 0.5f-void estimate()-{+void estimate() {applyGyro();applyAcc();signalizeHorizontality();}-void applyGyro()-{+void applyGyro() {// applying gyroattitude *= Quaternion::fromAngularRates(rates * dt);attitude.normalize();}-void applyAcc()-{+void applyAcc() {// test should we apply accelerometer gravity correctionfloat accNorm = acc.norm();bool landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;@@ -40,8 +37,7 @@ void applyAcc()attitude.normalize();}-void signalizeHorizontality()-{+void signalizeHorizontality() {float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));setLED(angle < 15 * DEG_TO_RAD);}diff --git a/flix/flix.ino b/flix/flix.inoindex ed7cdad..a8cb484 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -33,8 +33,7 @@ Vector acc; // accelerometer data, m/s/sQuaternion attitude; // estimated attitudefloat motors[4]; // normalized motors thrust in range [-1..1]-void setup()-{+void setup() {Serial.begin(SERIAL_BAUDRATE);Serial.println("Initializing flix");setupLED();@@ -50,8 +49,7 @@ void setup()Serial.println("Initializing complete");}-void loop()-{+void loop() {if (!readIMU()) return;step();diff --git a/flix/imu.ino b/flix/imu.inoindex 8f089f6..c3f511f 100644--- a/flix/imu.ino+++ b/flix/imu.ino@@ -11,8 +11,7 @@MPU9250 IMU(SPI, IMU_CS_PIN);-void setupIMU()-{+void setupIMU() {Serial.println("Setup IMU");auto status = IMU.begin();@@ -35,8 +34,7 @@ void setupIMU()// NOTE: very important, without the above the rate would be terrible 50 Hz}-bool readIMU()-{+bool readIMU() {if (IMU.readSensor() < 0) {Serial.println("IMU read error");return false;@@ -54,8 +52,7 @@ bool readIMU()return rates != lastRates;}-void calibrateGyro()-{+void calibrateGyro() {Serial.println("Calibrating gyro, stand still");delay(500);int status = IMU.calibrateGyro();@@ -64,8 +61,7 @@ void calibrateGyro()printIMUCal();}-void calibrateAccel()-{+void calibrateAccel() {Serial.println("Cal accel: place level"); delay(3000);IMU.calibrateAccel();Serial.println("Cal accel: place nose up"); delay(3000);@@ -81,24 +77,21 @@ void calibrateAccel()printIMUCal();}-void loadAccelCal()-{+void loadAccelCal() {// NOTE: this should be changed to the actual valuesIMU.setAccelCalX(-0.0048542023, 1.0008112192);IMU.setAccelCalY(0.0521845818, 0.9985780716);IMU.setAccelCalZ(0.5754694939, 1.0045746565);}-void loadGyroCal()-{+void loadGyroCal() {// NOTE: this should be changed to the actual valuesIMU.setGyroBiasX_rads(-0.0185128022);IMU.setGyroBiasY_rads(-0.0262369743);IMU.setGyroBiasZ_rads(0.0163032326);}-void printIMUCal()-{+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());diff --git a/flix/led.ino b/flix/led.inoindex f8ab8b0..30b79ce 100644--- a/flix/led.ino+++ b/flix/led.ino@@ -5,17 +5,14 @@#define BLINK_PERIOD 500000-void setupLED()-{+void setupLED() {pinMode(LED_BUILTIN, OUTPUT);}-void setLED(bool on)-{+void setLED(bool on) {digitalWrite(LED_BUILTIN, on ? HIGH : LOW);}-void blinkLED()-{+void blinkLED() {setLED(micros() / BLINK_PERIOD % 2);}diff --git a/flix/log.ino b/flix/log.inoindex cc360ca..e412cb4 100644--- a/flix/log.ino+++ b/flix/log.ino@@ -12,8 +12,7 @@float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)int logPointer = 0;-void logData()-{+void logData() {if (!armed) return;static float logTime = 0;@@ -41,8 +40,7 @@ void logData()}}-void dumpLog()-{+void dumpLog() {Serial.printf("t,rates.x,rates.y,rates.z,ratesTarget.x,ratesTarget.y,ratesTarget.z,""attitude.x,attitude.y,attitude.z,attitudeTarget.x,attitudeTarget.y,attitudeTarget.z,thrustTarget\n");for (int i = 0; i < LOG_SIZE; i++) {diff --git a/flix/lpf.h b/flix/lpf.hindex befa9a0..937078f 100644--- a/flix/lpf.h+++ b/flix/lpf.h@@ -6,8 +6,7 @@#pragma oncetemplate <typename T> // Using template to make the filter usable for scalar and vector values-class LowPassFilter-{+class LowPassFilter {public:float alpha; // smoothing constant, 1 means filter disabledT output;diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex 173c7b4..8dfff14 100644--- a/flix/mavlink.ino+++ b/flix/mavlink.ino@@ -11,8 +11,7 @@#define PERIOD_SLOW 1.0#define PERIOD_FAST 0.1-void sendMavlink()-{+void sendMavlink() {static float lastSlow = 0;static float lastFast = 0;@@ -55,8 +54,7 @@ void sendMavlink()}}-inline void sendMessage(const void *msg)-{+inline void sendMessage(const void *msg) {uint8_t buf[MAVLINK_MAX_PACKET_LEN];uint16_t len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);sendWiFi(buf, len);diff --git a/flix/motors.ino b/flix/motors.inoindex 8b8ee29..937d8e4 100644--- a/flix/motors.ino+++ b/flix/motors.ino@@ -39,8 +39,7 @@ void setupMotors() {Serial.println("Motors initialized");}-uint16_t getPWM(float val, int n)-{+uint16_t getPWM(float val, int n) {if (val == 0) {return PWM_NEUTRAL;} else if (val > 0) {@@ -54,16 +53,14 @@ uint8_t pwmToDutyCycle(uint16_t pwm) {return map(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);}-void sendMotors()-{+void sendMotors() {ledcWrite(0, pwmToDutyCycle(getPWM(motors[0], 0)));ledcWrite(1, pwmToDutyCycle(getPWM(motors[1], 1)));ledcWrite(2, pwmToDutyCycle(getPWM(motors[2], 2)));ledcWrite(3, pwmToDutyCycle(getPWM(motors[3], 3)));}-void fullMotorTest(int n, bool reverse)-{+void fullMotorTest(int n, bool reverse) {printf("Full test for motor %d\n", n);for (int pwm = PWM_NEUTRAL; pwm <= 2300 && pwm >= 700; pwm += reverse ? -100 : 100) {printf("Motor %d: %d\n", n, pwm);diff --git a/flix/pid.h b/flix/pid.hindex b26189f..a330349 100644--- a/flix/pid.h+++ b/flix/pid.h@@ -7,8 +7,7 @@#include "lpf.h"-class PID-{+class PID {public:float p = 0;float i = 0;diff --git a/flix/rc.ino b/flix/rc.inoindex 5a49563..536ecb0 100644--- a/flix/rc.ino+++ b/flix/rc.ino@@ -11,14 +11,12 @@ const uint16_t channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};SBUS RC(Serial2);-void setupRC()-{+void setupRC() {Serial.println("Setup RC");RC.begin();}-void readRC()-{+void readRC() {bool failSafe, lostFrame;if (RC.read(channels, &failSafe, &lostFrame)) {if (failSafe) { return; } // TODO:diff --git a/flix/time.ino b/flix/time.inoindex 2d15fa3..4632bfb 100644--- a/flix/time.ino+++ b/flix/time.ino@@ -3,8 +3,7 @@// Time related functions-void step()-{+void step() {float now = micros() / 1000000.0;dt = now - t;t = now;@@ -16,8 +15,7 @@ void step()computeLoopFreq();}-void computeLoopFreq()-{+void computeLoopFreq() {static float windowStart = 0;static uint32_t freq = 0;freq++;diff --git a/flix/util.ino b/flix/util.inoindex 13d7fc6..7d145ae 100644--- a/flix/util.ino+++ b/flix/util.ino@@ -5,29 +5,24 @@#include "math.h"-float mapf(long x, long in_min, long in_max, float out_min, float out_max)-{+float mapf(long x, long in_min, long in_max, float out_min, float out_max) {return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;}-float mapff(float x, float in_min, float in_max, float out_min, float out_max)-{+float mapff(float x, float in_min, float in_max, float out_min, float out_max) {return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;}-int8_t sign(float x)-{+int8_t sign(float x) {return (x > 0) - (x < 0);}-float randomFloat(float min, float max)-{+float randomFloat(float min, float max) {return min + (max - min) * (float)rand() / RAND_MAX;}// wrap angle to [-PI, PI)-float wrapAngle(float angle)-{+float wrapAngle(float angle) {angle = fmodf(angle, 2 * PI);if (angle > PI) {angle -= 2 * PI;diff --git a/flix/vector.h b/flix/vector.hindex 02779af..5ed57a5 100644--- a/flix/vector.h+++ b/flix/vector.h@@ -5,8 +5,7 @@#pragma once-class Vector : public Printable-{+class Vector : public Printable {public:float x, y, z;diff --git a/flix/wifi.ino b/flix/wifi.inoindex 8909edd..effb6f9 100644--- a/flix/wifi.ino+++ b/flix/wifi.ino@@ -18,15 +18,13 @@WiFiUDP udp;-void setupWiFi()-{+void setupWiFi() {Serial.println("Setup Wi-Fi");WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);IPAddress myIP = WiFi.softAPIP();}-inline void sendWiFi(const uint8_t *buf, size_t len)-{+inline void sendWiFi(const uint8_t *buf, size_t len) {udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT);udp.write(buf, len);udp.endPacket();diff --git a/gazebo/Arduino.h b/gazebo/Arduino.hindex bf85eb0..f17b8f9 100644--- a/gazebo/Arduino.h+++ b/gazebo/Arduino.h@@ -44,8 +44,7 @@ public:class Print {public:- size_t printf(const char *format, ...)- {+ size_t printf(const char *format, ...) {va_list args;va_start(args, format);size_t result = vprintf(format, args);@@ -53,48 +52,39 @@ public:return result;}- size_t print(float n, int digits = 2)- {+ size_t print(float n, int digits = 2) {return printf("%.*f", digits, n);}- size_t println(float n, int digits = 2)- {+ size_t println(float n, int digits = 2) {return printf("%.*f\n", digits, n);}- size_t print(const char* s)- {+ size_t print(const char* s) {return printf("%s", s);}- size_t println()- {+ size_t println() {return print("\n");}- size_t println(const char* s)- {+ size_t println(const char* s) {return printf("%s\n", s);}- size_t println(const Printable& p)- {+ size_t println(const Printable& p) {return p.printTo(*this) + print("\n");}- size_t print(const String& s)- {+ size_t print(const String& s) {return printf("%s", s.c_str());}- size_t println(const std::string& s)- {+ size_t println(const std::string& s) {return printf("%s\n", s.c_str());}- size_t println(const String& s)- {+ size_t println(const String& s) {return printf("%s\n", s.c_str());}};diff --git a/gazebo/joystick.h b/gazebo/joystick.hindex a275b2a..5ee5918 100644--- a/gazebo/joystick.h+++ b/gazebo/joystick.h@@ -24,8 +24,7 @@ bool joystickInitialized = false, warnShown = false;void normalizeRC();-void joystickInit()-{+void joystickInit() {SDL_Init(SDL_INIT_JOYSTICK);joystick = SDL_JoystickOpen(0);if (joystick != NULL) {@@ -37,8 +36,7 @@ void joystickInit()}}-void joystickGet()-{+void joystickGet() {if (!joystickInitialized) {joystickInit();return;diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cppindex fd5cecd..04ec3f2 100644--- a/gazebo/simulator.cpp+++ b/gazebo/simulator.cpp@@ -33,8 +33,7 @@ using ignition::math::Vector3d;using namespace gazebo;using namespace std;-class ModelFlix : public ModelPlugin-{+class ModelFlix : public ModelPlugin {private:physics::ModelPtr model;physics::LinkPtr body;@@ -45,8 +44,7 @@ private:LowPassFilter<Vector> accFilter = LowPassFilter<Vector>(0.1);public:- void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)- {+ void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) {this->model = _parent;this->body = this->model->GetLink("body");this->imu = dynamic_pointer_cast<sensors::ImuSensor>(sensors::get_sensor(model->GetScopedName(true) + "::body::imu")); // default::flix::body::imu@@ -58,14 +56,12 @@ public:}public:- void OnReset()- {+ void OnReset() {attitude = Quaternion(); // reset estimated attitudegzmsg << "Flix plugin reset" << endl;}- void OnUpdate()- {+ void OnUpdate() {__micros = model->GetWorld()->SimTime().Double() * 1000000;step();@@ -91,8 +87,7 @@ public:logData();}- void applyMotorsThrust()- {+ void applyMotorsThrust() {// thrustsconst double dist = 0.035355; // motors shift from the center, mconst double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78