Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Change C++ code style: put curly brace on the same line

Oleg Kalachevd2296fea76b4
diff --git a/flix/cli.ino b/flix/cli.ino
index 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.ino
index 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.ino
index 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 gyro
attitude *= Quaternion::fromAngularRates(rates * dt);
attitude.normalize();
}
-void applyAcc()
-{
+void applyAcc() {
// test should we apply accelerometer gravity correction
float 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.ino
index ed7cdad..a8cb484 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -33,8 +33,7 @@ Vector acc; // accelerometer data, m/s/s
Quaternion attitude; // estimated attitude
float 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.ino
index 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 values
IMU.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 values
IMU.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.ino
index 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.ino
index 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.h
index befa9a0..937078f 100644
--- a/flix/lpf.h
+++ b/flix/lpf.h
@@ -6,8 +6,7 @@
#pragma once
template <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 disabled
T output;
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 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.ino
index 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.h
index 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.ino
index 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.ino
index 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.ino
index 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.h
index 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.ino
index 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.h
index 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.h
index 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.cpp
index 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 attitude
gzmsg << "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() {
// thrusts
const double dist = 0.035355; // motors shift from the center, m
const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78