Make .cpp style version compile

.vscode/c_cpp_properties.json

@@ -18,20 +18,7 @@
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h"
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
@@ -66,19 +53,6 @@
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
@@ -113,20 +87,7 @@
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h"
-				"${workspaceFolder}/flix/cli.ino",
-				"${workspaceFolder}/flix/control.ino",
-				"${workspaceFolder}/flix/estimate.ino",
-				"${workspaceFolder}/flix/flix.ino",
-				"${workspaceFolder}/flix/imu.ino",
-				"${workspaceFolder}/flix/led.ino",
-				"${workspaceFolder}/flix/log.ino",
-				"${workspaceFolder}/flix/mavlink.ino",
-				"${workspaceFolder}/flix/motors.ino",
-				"${workspaceFolder}/flix/rc.ino",
-				"${workspaceFolder}/flix/time.ino",
-				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",

flix/cli.cpp

@@ -3,6 +3,8 @@
 
 // Implementation of command line interface
 
+#include <Arduino.h>
+#include "flix.h"
 #include "pid.h"
 #include "vector.h"
 #include "util.h"
@@ -11,7 +13,6 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO
 extern const int ACRO, STAB, AUTO;
 extern float t, dt, loopRate;
 extern uint16_t channels[16];
-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
 extern int mode;
 extern bool armed;
 
@@ -70,7 +71,7 @@ void pause(float duration) {
 	}
 }
 
-void doCommand(String str, bool echo = false) {
+void doCommand(String str, bool echo) {
 	// parse command
 	String command, arg0, arg1;
 	splitString(str, command, arg0, arg1);

flix/config.h

@@ -0,0 +1 @@
+#define WIFI_ENABLED 1

flix/control.cpp

@@ -34,9 +34,19 @@
 #define TILT_MAX radians(30)
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
+extern const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
+extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
+
 int mode = STAB;
 bool armed = false;
+float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
+float controlTime;
+
+Quaternion attitudeTarget;
+Vector ratesTarget;
+Vector ratesExtra; // feedforward rates
+Vector torqueTarget;
+float thrustTarget;
 
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
 PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
@@ -47,15 +57,6 @@ PID yawPID(YAW_P, 0, 0);
 Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
 float tiltMax = TILT_MAX;
 
-Quaternion attitudeTarget;
-Vector ratesTarget;
-Vector ratesExtra; // feedforward rates
-Vector torqueTarget;
-float thrustTarget;
-
-extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
-
 void control() {
 	interpretControls();
 	failsafe();

flix/estimate.cpp

@@ -3,6 +3,7 @@
 
 // Attitude estimation from gyro and accelerometer
 
+#include "flix.h"
 #include "quaternion.h"
 #include "vector.h"
 #include "lpf.h"
@@ -11,6 +12,10 @@
 #define WEIGHT_ACC 0.003
 #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
+Vector rates; // filtered angular rates, rad/s
+Quaternion attitude; // estimated attitude
+bool landed; // are we landed and stationary
+
 void estimate() {
 	applyGyro();
 	applyAcc();

flix/flix.h

@@ -0,0 +1,92 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// All-in-one header file
+
+#pragma once
+
+#include <Arduino.h>
+#include "vector.h"
+#include "quaternion.h"
+
+// The most used global variables:
+extern float t; // current step time, s
+extern float dt; // time delta from previous step, s
+extern Vector gyro; // gyroscope data
+extern Vector acc; // accelerometer data, m/s²
+extern Vector rates; // filtered angular rates, rad/s
+extern Quaternion attitude; // estimated attitude
+extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode; // pilot inputs, range [-1, 1]
+extern float controlTime; // inputs last update time
+extern int mode;
+extern bool armed;
+extern Vector ratesTarget, ratesExtra, torqueTarget;
+extern Quaternion attitudeTarget;
+extern float thrustTarget;
+extern bool landed; // are we landed and stationary
+extern float motors[4]; // normalized motors thrust in range [0..1]
+
+void print(const char* format, ...);
+void pause(float duration);
+void doCommand(String str, bool echo = false);
+void handleInput();
+void control();
+void interpretControls();
+void controlAttitude();
+void controlRates();
+void controlTorque();
+const char *getModeName();
+void estimate();
+void applyGyro();
+void applyAcc();
+void setupIMU();
+void configureIMU();
+void readIMU();
+void rotateIMU(Vector& data);
+void calibrateGyroOnce();
+void calibrateAccel();
+void calibrateAccelOnce();
+void printIMUCalibration();
+void printIMUInfo();
+void setupLED();
+void setLED(bool on);
+void blinkLED();
+void prepareLogData();
+void logData();
+void dumpLog();
+void processMavlink();
+void sendMavlink();
+void sendMessage(const void *msg);
+void receiveMavlink();
+void handleMavlink(const void *_msg);
+void mavlinkPrint(const char* str);
+void sendMavlinkPrint();
+void setupMotors();
+int getDutyCycle(float value);
+void sendMotors();
+bool motorsActive();
+void testMotor(int n);
+void setupParameters();
+int parametersCount();
+const char *getParameterName(int index);
+float getParameter(int index);
+float getParameter(const char *name);
+bool setParameter(const char *name, const float value);
+void syncParameters();
+void printParameters();
+void resetParameters();
+void setupRC();
+bool readRC();
+void normalizeRC();
+void calibrateRC();
+void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str);
+void printRCCalibration();
+void failsafe();
+void rcLossFailsafe();
+void descend();
+void autoFailsafe();
+void step();
+void computeLoopRate();
+void setupWiFi();
+void sendWiFi(const uint8_t *buf, int len);
+int receiveWiFi(uint8_t *buf, int len);

flix/flix.ino

@@ -3,26 +3,14 @@
 
 // Main firmware file
 
+#include "config.h"
 #include "vector.h"
 #include "quaternion.h"
 #include "util.h"
-
+#include "flix.h"
-#define SERIAL_BAUDRATE 115200
-#define WIFI_ENABLED 1
-
-float t = NAN; // current step time, s
-float dt; // time delta from previous step, s
-float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
-float controlMode = NAN;
-Vector gyro; // gyroscope data
-Vector acc; // accelerometer data, m/s/s
-Vector rates; // filtered angular rates, rad/s
-Quaternion attitude; // estimated attitude
-bool landed; // are we landed and stationary
-float motors[4]; // normalized motors thrust in range [0..1]
 
 void setup() {
-	Serial.begin(SERIAL_BAUDRATE);
+	Serial.begin(115200);
 	print("Initializing flix\n");
 	disableBrownOut();
 	setupParameters();

flix/imu.cpp

@@ -5,14 +5,19 @@
 
 #include <SPI.h>
 #include <FlixPeriph.h>
+#include "vector.h"
 #include "lpf.h"
 #include "util.h"
 
 MPU9250 imu(SPI);
 
-Vector accBias;
+Vector gyro; // gyroscope data
-Vector accScale(1, 1, 1);
+Vector gyroBias; // gyroscope bias
-Vector gyroBias;
+Vector acc; // accelerometer data, m/s/s
+Vector accBias; // accelerometer bias
+Vector accScale(1, 1, 1); // accelerometer scale
+
+extern float loopRate;
 
 void setupIMU() {
 	print("Setup IMU\n");

flix/led.cpp

@@ -3,6 +3,8 @@
 
 // Board's LED control
 
+#include <Arduino.h>
+
 #define BLINK_PERIOD 500000
 
 #ifndef LED_BUILTIN

flix/log.cpp

@@ -3,6 +3,7 @@
 
 // In-RAM logging
 
+#include "flix.h"
 #include "vector.h"
 
 #define LOG_RATE 100

flix/lpf.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include <Arduino.h>
+
 template <typename T> // Using template to make the filter usable for scalar and vector values
 class LowPassFilter {
 public:

flix/mavlink.cpp

@@ -3,6 +3,10 @@
 
 // MAVLink communication
 
+#include <Arduino.h>
+#include "flix.h"
+#include "config.h"
+
 #if WIFI_ENABLED
 
 #include <MAVLink.h>
@@ -14,8 +18,10 @@
 
 String mavlinkPrintBuffer;
 
+extern uint16_t channels[16];
 extern float controlTime;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
+extern const int STAB, AUTO;
 
 void processMavlink() {
 	sendMavlink();

flix/motors.cpp

@@ -4,6 +4,8 @@
 // Motors output control using MOSFETs
 // In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400)
 
+#include <Arduino.h>
+#include "flix.h"
 #include "util.h"
 
 #define MOTOR_0_PIN 12 // rear left
@@ -17,11 +19,11 @@
 #define PWM_MIN 0
 #define PWM_MAX 1000000 / PWM_FREQUENCY
 
-// Motors array indexes:
+float motors[4]; // normalized motors thrust in range [0..1]
-const int MOTOR_REAR_LEFT = 0;
+extern const int MOTOR_REAR_LEFT = 0;
-const int MOTOR_REAR_RIGHT = 1;
+extern const int MOTOR_REAR_RIGHT = 1;
-const int MOTOR_FRONT_RIGHT = 2;
+extern const int MOTOR_FRONT_RIGHT = 2;
-const int MOTOR_FRONT_LEFT = 3;
+extern const int MOTOR_FRONT_LEFT = 3;
 
 void setupMotors() {
 	print("Setup Motors\n");

flix/parameters.cpp

@@ -4,11 +4,17 @@
 // Parameters storage in flash memory
 
 #include <Preferences.h>
+#include "flix.h"
+#include "pid.h"
 
 extern float channelZero[16];
 extern float channelMax[16];
 extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
-extern float mavlinkControlScale;
+extern float tiltMax;
+extern PID rollPID, pitchPID, yawPID;
+extern PID rollRatePID, pitchRatePID, yawRatePID;
+extern Vector maxRate;
+extern Vector accBias, accScale;
 
 Preferences storage;
 

flix/pid.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include "Arduino.h"
+#include "flix.h"
 #include "lpf.h"
 
 class PID {

flix/quaternion.h

@@ -5,6 +5,7 @@
 
 #pragma once
 
+#include <Arduino.h>
 #include "vector.h"
 
 class Quaternion : public Printable {

flix/rc.cpp

@@ -9,7 +9,6 @@
 SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
 
 uint16_t channels[16]; // raw rc channels
-float controlTime; // time of the last controls update
 float channelZero[16]; // calibration zero values
 float channelMax[16]; // calibration max values
 

flix/safety.cpp

@@ -3,11 +3,13 @@
 
 // Fail-safe functions
 
+#include "flix.h"
+
 #define RC_LOSS_TIMEOUT 1
 #define DESCEND_TIME 10
 
 extern float controlTime;
-extern float controlRoll, controlPitch, controlThrottle, controlYaw;
+extern const int AUTO, STAB;
 
 void failsafe() {
 	rcLossFailsafe();

flix/time.cpp

@@ -3,6 +3,11 @@
 
 // Time related functions
 
+#include "Arduino.h"
+#include "flix.h"
+
+float t = NAN; // current step time, s
+float dt; // time delta from previous step, s
 float loopRate; // Hz
 
 void step() {

flix/util.h

@@ -8,28 +8,28 @@
 #include <math.h>
 #include <soc/soc.h>
 #include <soc/rtc_cntl_reg.h>
+#include "flix.h"
 
 const float ONE_G = 9.80665;
-extern float t;
 
-float mapf(long x, long in_min, long in_max, float out_min, float out_max) {
+inline 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) {
+inline 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;
 }
 
-bool invalid(float x) {
+inline bool invalid(float x) {
 	return !isfinite(x);
 }
 
-bool valid(float x) {
+inline bool valid(float x) {
 	return isfinite(x);
 }
 
 // Wrap angle to [-PI, PI)
-float wrapAngle(float angle) {
+inline float wrapAngle(float angle) {
 	angle = fmodf(angle, 2 * PI);
 	if (angle > PI) {
 		angle -= 2 * PI;
@@ -40,12 +40,12 @@ float wrapAngle(float angle) {
 }
 
 // Disable reset on low voltage
-void disableBrownOut() {
+inline void disableBrownOut() {
 	REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
 }
 
 // Trim and split string by spaces
-void splitString(String& str, String& token0, String& token1, String& token2) {
+inline void splitString(String& str, String& token0, String& token1, String& token2) {
 	str.trim();
 	char chars[str.length() + 1];
 	str.toCharArray(chars, str.length() + 1);

flix/vector.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include <Arduino.h>
+
 class Vector : public Printable {
 public:
 	float x, y, z;
@@ -124,5 +126,5 @@ public:
 	}
 };
 
-Vector operator * (const float a, const Vector& b) { return b * a; }
+inline Vector operator * (const float a, const Vector& b) { return b * a; }
-Vector operator + (const float a, const Vector& b) { return b + a; }
+inline Vector operator + (const float a, const Vector& b) { return b + a; }

flix/wifi.cpp

@@ -3,6 +3,9 @@
 
 // Wi-Fi support
 
+#include "config.h"
+#include "flix.h"
+
 #if WIFI_ENABLED
 
 #include <WiFi.h>