Apply motors configuration without reboot

docs/usage.md

@@ -143,34 +143,30 @@ If using brushless motors and ESCs:
 1. Set the appropriate PWM using the parameters: `MOT_PWM_STOP`, `MOT_PWM_MIN`, and `MOT_PWM_MAX` (1000, 1000, and 2000 is typical).
 2. Decrease the PWM frequency using the `MOT_PWM_FREQ` parameter (400 is typical).
 
-Reboot the drone to apply the changes.
-
 > [!CAUTION]
 > **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them.
 
-### Important: check everything works
+### Check everything works
 
-1. Check the IMU is working: perform `imu` command in the console and check the output:
+1. Check the IMU is working: perform `imu` command and check its output:
 
    * The `status` field should be `OK`.
    * The `rate` field should be about 1000 (Hz).
    * The `accel` and `gyro` fields should change as you move the drone.
-   * The `accel bias` and `accel scale` fields should contain calibration parameters (not zeros and ones).
-   * The `gyro bias` field should contain estimated gyro bias (not zeros).
    * The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up.
 
 2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Compare your attitude indicator (in the *large vertical* mode) to the video:
 
     <a href="https://youtu.be/yVRN23-GISU"><img width=300 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a>
 
-3. Perform motor tests. Use the following commands **— remove the propellers before running the tests!**
+3. Perform motor tests in the console. Use the following commands **— remove the propellers before running the tests!**
 
-   * `mfr` — rotate front right motor (counter-clockwise).
-   * `mfl` — rotate front left motor (clockwise).
-   * `mrl` — rotate rear left motor (counter-clockwise).
-   * `mrr` — rotate rear right motor (clockwise).
+   * `mfr` — should rotate front right motor (counter-clockwise).
+   * `mfl` — should rotate front left motor (clockwise).
+   * `mrl` — should rotate rear left motor (counter-clockwise).
+   * `mrr` — should rotate rear right motor (clockwise).
 
-   Make sure rotation directions and propeller types match the following diagram:
+   Rotation diagram:
 
    <img src="img/motors.svg" width=200>
 
@@ -236,11 +232,11 @@ When finished flying, **disarm** the drone, moving the left stick to the bottom
 
 ### Flight modes
 
-Flight mode is changed using mode switch on the remote control (if configured) or using the console commands. The main flight mode is *STAB*.
+Flight mode is changed using mode switch on the remote control or using the command line.
 
 #### STAB
 
-In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
+The default mode is *STAB*. In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
 
 > [!IMPORTANT]
 > The drone doesn't stabilize its position, so slight drift is possible. The pilot should compensate it manually.
@@ -255,7 +251,7 @@ In this mode, the pilot controls the angular rates. This control method is diffi
 
 #### AUTO
 
-In this mode, the pilot inputs are ignored (except the mode switch). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed behavior.
+In this mode, the pilot inputs are ignored (except the mode switch, if configured). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed autonomous behavior.
 
 If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
 

docs/user.md

@@ -4,33 +4,6 @@ This page contains user-built drones based on the Flix project. Publish your pro
 
 ---
 
-Author: [FanBy0ru](https://https://github.com/FanBy0ru).<br>
-Description: custom 3D-printed frame.<br>
-Frame STLs and flight validation: https://cults3d.com/en/3d-model/gadget/armature-pour-flix-drone.
-
-<img src="img/user/fanby0ru/1.jpg" height=200> <img src="img/user/fanby0ru/2.jpg" height=200>
-
----
-
-Author: Ivan44 Phalko.<br>
-Description: custom PCB, cusom test bench.<br>
-[Flight validation](https://drive.google.com/file/d/17DNDJ1gPmCmDRAwjedCbJ9RXAyqMqqcX/view?usp=sharing).
-
-<img src="img/user/phalko/1.jpg" height=200> <img src="img/user/phalko/2.jpg" height=200> <img src="img/user/phalko/3.jpg" height=200>
-
----
-
-Author: **Arkadiy "Arky" Matsekh**, Foucault Dynamics, Gold Coast, Australia.<br>
-The drone was built for the University of Queensland industry-led Master's capstone project.
-
-**Flight video:**
-
-<a href="https://drive.google.com/file/d/1NNYSVXBY-w0JjCo07D8-PgnVq3ca9plj/view?usp=sharing"><img height=300 src="img/user/arkymatsekh/video.jpg"></a>
-
-<img src="img/user/arkymatsekh/1.jpg" height=150> <img src="img/user/arkymatsekh/2.jpg" height=150> <img src="img/user/arkymatsekh/3.jpg" height=150>
-
----
-
 Author: [goldarte](https://t.me/goldarte).<br>
 
 <img src="img/user/goldarte/1.jpg" height=150> <img src="img/user/goldarte/2.jpg" height=150>

flix/control.ino

@@ -52,7 +52,6 @@ PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
 PID yawPID(YAW_P, 0, 0);
 Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
 float tiltMax = TILT_MAX;
-int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
@@ -66,9 +65,9 @@ void control() {
 }
 
 void interpretControls() {
-	if (controlMode < 0.25) mode = flightModes[0];
-	else if (controlMode < 0.75) mode = flightModes[1];
-	else if (controlMode > 0.75) mode = flightModes[2];
+	if (controlMode < 0.25) mode = STAB;
+	if (controlMode < 0.75) mode = STAB;
+	if (controlMode > 0.75) mode = STAB;
 
 	if (mode == AUTO) return; // pilot is not effective in AUTO mode
 

flix/estimate.ino

@@ -1,7 +1,7 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Attitude estimation using gyro and accelerometer
+// Attitude estimation from gyro and accelerometer
 
 #include "quaternion.h"
 #include "vector.h"

flix/flix.ino

@@ -21,8 +21,8 @@ void setup() {
 	disableBrownOut();
 	setupParameters();
 	setupLED();
-	setLED(true);
 	setupMotors();
+	setLED(true);
 	setupWiFi();
 	setupIMU();
 	setupRC();

flix/mavlink.ino

@@ -8,13 +8,12 @@
 
 extern float controlTime;
 
+bool mavlinkConnected = false;
+String mavlinkPrintBuffer;
 int mavlinkSysId = 1;
 Rate telemetryFast(10);
 Rate telemetrySlow(2);
 
-bool mavlinkConnected = false;
-String mavlinkPrintBuffer;
-
 void processMavlink() {
 	sendMavlink();
 	receiveMavlink();
@@ -42,9 +41,9 @@ void sendMavlink() {
 	}
 
 	if (telemetryFast && mavlinkConnected) {
-		const float offset[] = {0, 0, 0, 0};
+		const float zeroQuat[] = {0, 0, 0, 0};
 		mavlink_msg_attitude_quaternion_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
-			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, offset); // convert to frd
+			time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
 		sendMessage(&msg);
 
 		mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,

flix/motors.ino

@@ -24,7 +24,7 @@ void setupMotors() {
 	// configure pins
 	for (int i = 0; i < 4; i++) {
 		ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
-		pwmFrequency = ledcChangeFrequency(motorPins[i], pwmFrequency, pwmResolution); // when reconfiguring
+		pwmFrequency = ledcChangeFrequency(motorPins[i], pwmFrequency, pwmResolution); // if re-initializing
 	}
 	sendMotors();
 	print("Motors initialized\n");

flix/parameters.ino

@@ -6,22 +6,21 @@
 #include <Preferences.h>
 #include "util.h"
 
-extern int channelZero[16];
-extern int channelMax[16];
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern float channelZero[16];
+extern float channelMax[16];
+extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
 extern int wifiMode, udpLocalPort, udpRemotePort;
 extern float rcLossTimeout, descendTime;
 
 Preferences storage;
 
 struct Parameter {
-	const char *name; // max length is 15
+	const char *name; // max length is 15 (Preferences key limit)
 	bool integer;
-	union { float *f; int *i; }; // pointer to the variable
+	union { float *f; int *i; }; // pointer to variable
 	float cache; // what's stored in flash
-	void (*callback)(); // called after parameter change
-	Parameter(const char *name, float *variable, void (*callback)() = nullptr) : name(name), integer(false), f(variable), callback(callback) {};
-	Parameter(const char *name, int *variable, void (*callback)() = nullptr) : name(name), integer(true), i(variable), callback(callback) {};
+	Parameter(const char *name, float *variable) : name(name), integer(false), f(variable) {};
+	Parameter(const char *name, int *variable) : name(name), integer(true), i(variable) {};
 	float getValue() const { return integer ? *i : *f; };
 	void setValue(const float value) { if (integer) *i = value; else *f = value; };
 };
@@ -50,9 +49,6 @@ Parameter parameters[] = {
 	{"CTL_R_RATE_MAX", &maxRate.x},
 	{"CTL_Y_RATE_MAX", &maxRate.z},
 	{"CTL_TILT_MAX", &tiltMax},
-	{"CTL_FLT_MODE_0", &flightModes[0]},
-	{"CTL_FLT_MODE_1", &flightModes[1]},
-	{"CTL_FLT_MODE_2", &flightModes[2]},
 	// imu
 	{"IMU_ROT_ROLL", &imuRotation.x},
 	{"IMU_ROT_PITCH", &imuRotation.y},
@@ -68,12 +64,12 @@ Parameter parameters[] = {
 	{"EST_ACC_WEIGHT", &accWeight},
 	{"EST_RATES_LPF_A", &ratesFilter.alpha},
 	// motors
-	{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},
-	{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT], setupMotors},
-	{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT], setupMotors},
-	{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT], setupMotors},
-	{"MOT_PWM_FREQ", &pwmFrequency, setupMotors},
-	{"MOT_PWM_RES", &pwmResolution, setupMotors},
+	{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT]},
+	{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT]},
+	{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT]},
+	{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT]},
+	{"MOT_PWM_FREQ", &pwmFrequency},
+	{"MOT_PWM_RES", &pwmResolution},
 	{"MOT_PWM_STOP", &pwmStop},
 	{"MOT_PWM_MIN", &pwmMin},
 	{"MOT_PWM_MAX", &pwmMax},
@@ -113,8 +109,7 @@ Parameter parameters[] = {
 };
 
 void setupParameters() {
-	print("Setup parameters\n");
-	storage.begin("flix");
+	storage.begin("flix", false);
 	// Read parameters from storage
 	for (auto &parameter : parameters) {
 		if (!storage.isKey(parameter.name)) {
@@ -125,6 +120,11 @@ void setupParameters() {
 	}
 }
 
+void afterParameterChange(String name, const float value) {
+	if (name == "MOT_PWM_FREQ" || name == "MOT_PWM_RES") setupMotors();
+	if (name == "MOT_PIN_FL" || name == "MOT_PIN_FR" || name == "MOT_PIN_RL" || name == "MOT_PIN_RR") setupMotors();
+}
+
 int parametersCount() {
 	return sizeof(parameters) / sizeof(parameters[0]);
 }
@@ -153,7 +153,7 @@ bool setParameter(const char *name, const float value) {
 		if (strcasecmp(parameter.name, name) == 0) {
 			if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
 			parameter.setValue(value);
-			if (parameter.callback) parameter.callback();
+			afterParameterChange(name, value);
 			return true;
 		}
 	}
@@ -167,7 +167,8 @@ void syncParameters() {
 
 	for (auto &parameter : parameters) {
 		if (parameter.getValue() == parameter.cache) continue; // no change
-		if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NAN
+		if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NaN
+		if (isinf(parameter.getValue()) && isinf(parameter.cache)) continue; // both are Inf
 
 		storage.putFloat(parameter.name, parameter.getValue());
 		parameter.cache = parameter.getValue(); // update cache

flix/rc.ino

@@ -9,14 +9,15 @@
 SBUS rc(Serial2);
 
 uint16_t channels[16]; // raw rc channels
-int channelZero[16]; // calibration zero values
-int channelMax[16]; // calibration max values
+float channelZero[16]; // calibration zero values
+float channelMax[16]; // calibration max values
 
 float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
 float controlMode = NAN;
 float controlTime = NAN; // time of the last controls update
 
-int rollChannel = -1, pitchChannel = -1, throttleChannel = -1, yawChannel = -1, modeChannel = -1; // channel mapping
+// Channels mapping (nan means not assigned):
+float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
 
 void setupRC() {
 	print("Setup RC\n");
@@ -40,11 +41,11 @@ void normalizeRC() {
 		controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
 	}
 	// Update control values
-	controlRoll = rollChannel < 0 ? 0 : controls[rollChannel];
-	controlPitch = pitchChannel < 0 ? 0 : controls[pitchChannel];
-	controlYaw = yawChannel < 0 ? 0 : controls[yawChannel];
-	controlThrottle = throttleChannel < 0 ? 0 : controls[throttleChannel];
-	controlMode = modeChannel < 0 ? NAN : controls[modeChannel]; // mode control is ineffective if not mapped
+	controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
+	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
+	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
+	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
+	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
 }
 
 void calibrateRC() {
@@ -63,7 +64,7 @@ void calibrateRC() {
 	printRCCalibration();
 }
 
-void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const char *str) {
+void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str) {
 	print("%s", str);
 	pause(3);
 	for (int i = 0; i < 30; i++) readRC(); // try update 30 times max
@@ -84,15 +85,15 @@ void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const c
 		channelZero[ch] = in[ch];
 		channelMax[ch] = out[ch];
 	} else {
-		*channel = -1;
+		*channel = NAN;
 	}
 }
 
 void printRCCalibration() {
 	print("Control   Ch     Zero   Max\n");
-	print("Roll      %-7d%-7d%-7d\n", rollChannel, rollChannel < 0 ? 0 : channelZero[rollChannel], rollChannel < 0 ? 0 : channelMax[rollChannel]);
-	print("Pitch     %-7d%-7d%-7d\n", pitchChannel, pitchChannel < 0 ? 0 : channelZero[pitchChannel], pitchChannel < 0 ? 0 : channelMax[pitchChannel]);
-	print("Yaw       %-7d%-7d%-7d\n", yawChannel, yawChannel < 0 ? 0 : channelZero[yawChannel], yawChannel < 0 ? 0 : channelMax[yawChannel]);
-	print("Throttle  %-7d%-7d%-7d\n", throttleChannel, throttleChannel < 0 ? 0 : channelZero[throttleChannel], throttleChannel < 0 ? 0 : channelMax[throttleChannel]);
-	print("Mode      %-7d%-7d%-7d\n", modeChannel, modeChannel < 0 ? 0 : channelZero[modeChannel], modeChannel < 0 ? 0 : channelMax[modeChannel]);
+	print("Roll      %-7g%-7g%-7g\n", rollChannel, rollChannel >= 0 ? channelZero[(int)rollChannel] : NAN, rollChannel >= 0 ? channelMax[(int)rollChannel] : NAN);
+	print("Pitch     %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);
+	print("Yaw       %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);
+	print("Throttle  %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);
+	print("Mode      %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
 }

gazebo/flix.h

@@ -9,7 +9,6 @@
 #include "quaternion.h"
 #include "Arduino.h"
 #include "wifi.h"
-#include "lpf.h"
 
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -20,7 +19,6 @@ extern float motors[4];
 
 Vector gyro, acc, imuRotation;
 Vector accBias, gyroBias, accScale(1, 1, 1);
-LowPassFilter<Vector> gyroBiasFilter(0);
 
 // declarations
 void step();
@@ -43,7 +41,7 @@ void doCommand(String str, bool echo);
 void handleInput();
 void normalizeRC();
 void calibrateRC();
-void calibrateRCChannel(int *channel, uint16_t zero[16], uint16_t max[16], const char *str);
+void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);
 void printRCCalibration();
 void printLogHeader();
 void printLogData();

gazebo/wifi.h

@@ -11,10 +11,9 @@
 #include <sys/poll.h>
 #include <gazebo/gazebo.hh>
 
-int wifiMode = 1; // mock
-int udpLocalPort = 14580;
-int udpRemotePort = 14550;
-const char *udpRemoteIP = "255.255.255.255";
+#define WIFI_UDP_PORT 14580
+#define WIFI_UDP_REMOTE_PORT 14550
+#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
 
 int wifiSocket;
 
@@ -23,22 +22,22 @@ void setupWiFi() {
 	sockaddr_in addr; // local address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_ANY;
-	addr.sin_port = htons(udpLocalPort);
+	addr.sin_port = htons(WIFI_UDP_PORT);
 	if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
-		gzerr << "Failed to bind WiFi UDP socket on port " << udpLocalPort << std::endl;
+		gzerr << "Failed to bind WiFi UDP socket on port " << WIFI_UDP_PORT << std::endl;
 		return;
 	}
 	int broadcast = 1;
 	setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
-	gzmsg << "WiFi UDP socket initialized on port " << udpLocalPort << " (remote port " << udpRemotePort << ")" << std::endl;
+	gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << std::endl;
 }
 
 void sendWiFi(const uint8_t *buf, int len) {
 	if (wifiSocket == 0) setupWiFi();
 	sockaddr_in addr; // remote address
 	addr.sin_family = AF_INET;
-	addr.sin_addr.s_addr = inet_addr(udpRemoteIP);
-	addr.sin_port = htons(udpRemotePort);
+	addr.sin_addr.s_addr = inet_addr(WIFI_UDP_REMOTE_ADDR);
+	addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
 	sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
 }
 

tools/pyflix/flix.py

@@ -138,7 +138,7 @@ class Flix:
         while True:
             try:
                 msg: Optional[mavlink.MAVLink_message] = self.connection.recv_match(blocking=True)
-                if msg is None or msg.get_srcSystem() != self.system_id:
+                if msg is None:
                     continue
                 self._connected()
                 msg_dict = msg.to_dict()