Another minor fixes

By default the WiFi channel is used

.vscode/c_cpp_properties.json

@@ -17,7 +17,21 @@
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h"
+				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/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",
+				"${workspaceFolder}/flix/safety.ino"
 			],
 			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
@@ -50,7 +64,21 @@
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h"
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/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",
+				"${workspaceFolder}/flix/safety.ino"
 			],
 			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
@@ -84,7 +112,21 @@
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
 				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h"
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/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",
+				"${workspaceFolder}/flix/safety.ino"
 			],
 			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",

flix/cli.ino

@@ -3,8 +3,6 @@
 
 // Implementation of command line interface
 
-#include <Arduino.h>
-#include "flix.h"
 #include "pid.h"
 #include "vector.h"
 #include "util.h"
@@ -78,7 +76,7 @@ void pause(float duration) {
 	}
 }
 
-void doCommand(String str, bool echo) {
+void doCommand(String str, bool echo = false) {
 	// parse command
 	String command, arg0, arg1;
 	splitString(str, command, arg0, arg1);

flix/config.h

@@ -1,55 +0,0 @@
-// Wi-Fi
-#define WIFI_ENABLED 1
-#define WIFI_SSID "flix"
-#define WIFI_PASSWORD "flixwifi"
-#define WIFI_UDP_PORT 14550
-#define WIFI_UDP_REMOTE_PORT 14550
-#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
-
-// Motors
-#define MOTOR_0_PIN 12 // rear left
-#define MOTOR_1_PIN 13 // rear right
-#define MOTOR_2_PIN 14 // front right
-#define MOTOR_3_PIN 15 // front left
-#define PWM_FREQUENCY 78000
-#define PWM_RESOLUTION 10
-#define PWM_STOP 0
-#define PWM_MIN 0
-#define PWM_MAX 1000000 / PWM_FREQUENCY
-
-// Control
-#define PITCHRATE_P 0.05
-#define PITCHRATE_I 0.2
-#define PITCHRATE_D 0.001
-#define PITCHRATE_I_LIM 0.3
-#define ROLLRATE_P PITCHRATE_P
-#define ROLLRATE_I PITCHRATE_I
-#define ROLLRATE_D PITCHRATE_D
-#define ROLLRATE_I_LIM PITCHRATE_I_LIM
-#define YAWRATE_P 0.3
-#define YAWRATE_I 0.0
-#define YAWRATE_D 0.0
-#define YAWRATE_I_LIM 0.3
-#define ROLL_P 6
-#define ROLL_I 0
-#define ROLL_D 0
-#define PITCH_P ROLL_P
-#define PITCH_I ROLL_I
-#define PITCH_D ROLL_D
-#define YAW_P 3
-#define PITCHRATE_MAX radians(360)
-#define ROLLRATE_MAX radians(360)
-#define YAWRATE_MAX radians(300)
-#define TILT_MAX radians(30)
-#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
-
-// Estimation
-#define WEIGHT_ACC 0.003
-#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
-
-// MAVLink
-#define SYSTEM_ID 1
-
-// Safety
-#define RC_LOSS_TIMEOUT 1
-#define DESCEND_TIME 10

flix/control.ino

@@ -3,16 +3,38 @@
 
 // Flight control
 
-#include "config.h"
-#include "flix.h"
 #include "vector.h"
 #include "quaternion.h"
 #include "pid.h"
 #include "lpf.h"
 #include "util.h"
 
-extern const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
+#define PITCHRATE_P 0.05
+#define PITCHRATE_I 0.2
+#define PITCHRATE_D 0.001
+#define PITCHRATE_I_LIM 0.3
+#define ROLLRATE_P PITCHRATE_P
+#define ROLLRATE_I PITCHRATE_I
+#define ROLLRATE_D PITCHRATE_D
+#define ROLLRATE_I_LIM PITCHRATE_I_LIM
+#define YAWRATE_P 0.3
+#define YAWRATE_I 0.0
+#define YAWRATE_D 0.0
+#define YAWRATE_I_LIM 0.3
+#define ROLL_P 6
+#define ROLL_I 0
+#define ROLL_D 0
+#define PITCH_P ROLL_P
+#define PITCH_I ROLL_I
+#define PITCH_D ROLL_D
+#define YAW_P 3
+#define PITCHRATE_MAX radians(360)
+#define ROLLRATE_MAX radians(360)
+#define YAWRATE_MAX radians(300)
+#define TILT_MAX radians(30)
+#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
+const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
 int mode = STAB;
 bool armed = false;
 

flix/estimate.ino

@@ -3,8 +3,6 @@
 
 // Attitude estimation using gyro and accelerometer
 
-#include "config.h"
-#include "flix.h"
 #include "quaternion.h"
 #include "vector.h"
 #include "lpf.h"

flix/flix.h

@@ -1,95 +0,0 @@
-// 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"
-
-extern float t, dt;
-extern float loopRate;
-extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
-extern Vector gyro, acc;
-extern Vector rates;
-extern Quaternion attitude;
-extern bool landed;
-extern int mode;
-extern bool armed;
-extern Quaternion attitudeTarget;
-extern Vector ratesTarget, ratesExtra, torqueTarget;
-extern float thrustTarget;
-extern float motors[4];
-
-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();
-void desaturate(float& a, float& b, float& c, float& d);
-const char *getModeName();
-void estimate();
-void applyGyro();
-void applyAcc();
-void applyLevel();
-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 printLogHeader();
-void printLogData();
-void processMavlink();
-void sendMavlink();
-void sendMessage(const void *msg);
-void receiveMavlink();
-void printWiFiInfo();
-void configWiFi(int mode, const char *first, const char *second);
-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(int *channel, uint16_t in[16], uint16_t out[16], const char *str);
-void printRCCalibration();
-void setupPower();
-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,11 +3,17 @@
 
 // Main firmware file
 
-#include "config.h"
 #include "vector.h"
 #include "quaternion.h"
 #include "util.h"
-#include "flix.h"
+
+extern float t, dt;
+extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
+extern Vector gyro, acc;
+extern Vector rates;
+extern Quaternion attitude;
+extern bool landed;
+extern float motors[4];
 
 void setup() {
 	Serial.begin(115200);

flix/led.ino

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

flix/log.ino

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

flix/lpf.h

@@ -5,8 +5,6 @@
 
 #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.ino

@@ -3,22 +3,18 @@
 
 // MAVLink communication
 
-#include <Arduino.h>
 #include <MAVLink.h>
-#include "config.h"
 #include "util.h"
 
-extern const int RAW, ACRO, STAB, AUTO;
 extern float controlTime;
 extern float voltage;
-extern uint16_t channels[16];
 
 int mavlinkSysId = 1;
 Rate telemetryFast(10);
 Rate telemetrySlow(2);
 
 bool mavlinkConnected = false;
-static String mavlinkPrintBuffer;
+String mavlinkPrintBuffer;
 
 void processMavlink() {
 	sendMavlink();
@@ -215,7 +211,6 @@ void handleMavlink(const void *_msg) {
 		armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
 	}
 
-	/* TODO:
 	if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
 		mavlink_log_request_data_t m;
 		mavlink_msg_log_request_data_decode(&msg, &m);
@@ -229,7 +224,6 @@ void handleMavlink(const void *_msg) {
 			sendMessage(&msg);
 		}
 	}
-	*/
 
 	// Handle commands
 	if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {

flix/motors.ino

@@ -3,9 +3,6 @@
 
 // PWM control for motors
 
-#include <Arduino.h>
-#include "config.h"
-#include "flix.h"
 #include "util.h"
 
 float motors[4]; // normalized motor thrusts in range [0..1]
@@ -17,7 +14,10 @@ int pwmStop = 0;
 int pwmMin = 0;
 int pwmMax = -1; // -1 means duty cycle mode
 
-extern const int MOTOR_REAR_LEFT = 0, MOTOR_REAR_RIGHT = 1, MOTOR_FRONT_RIGHT = 2, MOTOR_FRONT_LEFT = 3;
+const int MOTOR_REAR_LEFT = 0;
+const int MOTOR_REAR_RIGHT = 1;
+const int MOTOR_FRONT_RIGHT = 2;
+const int MOTOR_FRONT_LEFT = 3;
 
 void setupMotors() {
 	print("Setup Motors\n");

flix/parameters.ino

@@ -4,32 +4,17 @@
 // Parameters storage in flash memory
 
 #include <Preferences.h>
-#include "flix.h"
-#include "pid.h"
-#include "lpf.h"
 #include "util.h"
 
-extern float channelZero[16], channelMax[16];
-extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
-extern float tiltMax;
-extern int flightModes[3];
-extern PID rollPID, pitchPID, yawPID;
-extern PID rollRatePID, pitchRatePID, yawRatePID;
-extern Vector maxRate;
-extern Vector imuRotation;
-extern Vector accBias, accScale;
-extern float accWeight, levelWeight;
-extern LowPassFilter<Vector> gyroBiasFilter, ratesFilter, voltageFilter;
-extern int rcRxPin, voltagePin;
-extern int motorPins[4];
-extern int pwmFrequency, pwmResolution, pwmStop, pwmMin, pwmMax;
+extern int channelZero[16];
+extern int channelMax[16];
+extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern int rcRxPin;
 extern int wifiMode, wifiLongRange, udpLocalPort, udpRemotePort, espnowChannel;
-extern int mavlinkSysId;
-extern Rate telemetrySlow, telemetryFast;
 extern float rcLossTimeout, descendTime;
 extern int voltagePin;
 extern float voltageScale;
-extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
+extern LowPassFilter<float> voltageFilter;
 
 Preferences storage;
 
@@ -45,7 +30,7 @@ struct Parameter {
 	void setValue(const float value) { if (integer) *i = value; else *f = value; };
 };
 
-static Parameter parameters[] = {
+Parameter parameters[] = {
 	// control
 	{"CTL_R_RATE_P", &rollRatePID.p},
 	{"CTL_R_RATE_I", &rollRatePID.i},

flix/pid.h

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

flix/quaternion.h

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

flix/rc.ino

@@ -6,7 +6,7 @@
 #include <SBUS.h>
 #include "util.h"
 
-static SBUS rc(Serial1);
+SBUS rc(Serial1);
 int rcRxPin = -1; // -1 means disabled
 
 uint16_t channels[16]; // raw rc channels

flix/safety.ino

@@ -3,12 +3,8 @@
 
 // Fail-safe functions
 
-#include "config.h"
-#include "flix.h"
-#include "util.h"
-
 extern float controlTime;
-extern const int AUTO, STAB;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
 float rcLossTimeout = 1;
 float descendTime = 10;

flix/time.ino

@@ -3,9 +3,6 @@
 
 // Time related functions
 
-#include "Arduino.h"
-#include "flix.h"
-
 float t = NAN; // current time, s
 float dt; // time delta with the previous step, s
 float loopRate; // Hz

flix/util.h

@@ -7,24 +7,24 @@
 
 #include <math.h>
 #include <ESP32_NOW_Serial.h>
-#include "flix.h"
 
 const float ONE_G = 9.80665;
+extern float t;
 
-inline float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
+float mapf(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;
 }
 
-inline bool invalid(float x) {
+bool invalid(float x) {
 	return !isfinite(x);
 }
 
-inline bool valid(float x) {
+bool valid(float x) {
 	return isfinite(x);
 }
 
 // Wrap angle to [-PI, PI)
-inline float wrapAngle(float angle) {
+float wrapAngle(float angle) {
 	angle = fmodf(angle, 2 * PI);
 	if (angle > PI) {
 		angle -= 2 * PI;
@@ -35,7 +35,7 @@ inline float wrapAngle(float angle) {
 }
 
 // Trim and split string by spaces
-inline void splitString(String& str, String& token0, String& token1, String& token2) {
+void splitString(String& str, String& token0, String& token1, String& token2) {
 	str.trim();
 	if (str.isEmpty()) return;
 	char chars[str.length() + 1];

flix/vector.h

@@ -5,8 +5,6 @@
 
 #pragma once
 
-#include <Arduino.h>
-
 class Vector : public Printable {
 public:
 	float x, y, z;
@@ -138,5 +136,5 @@ public:
 	}
 };
 
-inline 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; }
+Vector operator + (const float a, const Vector& b) { return b + a; }

flix/wifi.ino

@@ -8,25 +8,22 @@
 #include <WiFiUdp.h>
 #include <MacAddress.h>
 #include <ESP32_NOW_Serial.h>
-#include <Preferences.h>
-#include "config.h"
-#include "flix.h"
+#include "Preferences.h"
 #include "util.h"
 
 extern Preferences storage; // use the main preferences storage
-extern bool mavlinkConnected;
 
-extern const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
+const int W_DISABLED = 0, W_AP = 1, W_STA = 2, W_ESPNOW = 3;
 int wifiMode = W_AP;
 
 int wifiLongRange = 0;
 int udpLocalPort = 14550;
 int udpRemotePort = 14550;
-static IPAddress udpRemoteIP = "255.255.255.255";
-static WiFiUDP udp;
+IPAddress udpRemoteIP = "255.255.255.255";
+WiFiUDP udp;
 
-static ESPNOWSerial espnow(NULL, 0, WIFI_IF_AP);
-static ESPNOWSerial espnowBroadcast(ESP_NOW.BROADCAST_ADDR, 0, WIFI_IF_AP);
+ESPNOWSerial espnow(NULL, 0, WIFI_IF_AP);
+ESPNOWSerial espnowBroadcast(ESP_NOW.BROADCAST_ADDR, 0, WIFI_IF_AP);
 int espnowChannel = 6;
 
 void setupWiFi() {

gazebo/CMakeLists.txt

@@ -10,23 +10,9 @@ list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
 
 set(FLIX_SOURCE_DIR ../flix)
 include_directories(${FLIX_SOURCE_DIR})
-set(FLIX_SOURCE_DIR ../flix)
-include_directories(${FLIX_SOURCE_DIR})
-set(FLIX_SOURCES
-  ${FLIX_SOURCE_DIR}/cli.cpp
-  ${FLIX_SOURCE_DIR}/control.cpp
-  ${FLIX_SOURCE_DIR}/estimate.cpp
-  ${FLIX_SOURCE_DIR}/safety.cpp
-  ${FLIX_SOURCE_DIR}/log.cpp
-  ${FLIX_SOURCE_DIR}/mavlink.cpp
-  ${FLIX_SOURCE_DIR}/motors.cpp
-  ${FLIX_SOURCE_DIR}/parameters.cpp
-  ${FLIX_SOURCE_DIR}/rc.cpp
-  ${FLIX_SOURCE_DIR}/time.cpp
-)
 
 set(CMAKE_BUILD_TYPE RelWithDebInfo)
-add_library(flix SHARED simulator.cpp ${FLIX_SOURCES})
+add_library(flix SHARED simulator.cpp)
 target_link_libraries(flix ${GAZEBO_LIBRARIES} ${SDL2_LIBRARIES})
 target_include_directories(flix PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_compile_options(flix PRIVATE -Wno-address-of-packed-member) # disable unneeded mavlink warnings

gazebo/simulator.cpp

@@ -18,6 +18,19 @@
 #include "Arduino.h"
 #include "flix.h"
 
+#include "cli.ino"
+#include "control.ino"
+#include "estimate.ino"
+#include "safety.ino"
+#include "log.ino"
+#include "lpf.h"
+#include "mavlink.ino"
+#include "motors.ino"
+#include "parameters.ino"
+#include "power.ino"
+#include "rc.ino"
+#include "time.ino"
+
 using ignition::math::Vector3d;
 using namespace gazebo;
 using namespace std;