Add parameters for mavlink subsystem

System ID, fast telemetry rate, slow telemetry rate.

.github/workflows/build.yml

@@ -25,8 +25,6 @@ jobs:
         path: flix/build
     - name: Build firmware for ESP32-S3
       run: make BOARD=esp32:esp32:esp32s3
-    - name: Build firmware with WiFi disabled
-      run: sed -i 's/^#define WIFI_ENABLED 1$/#define WIFI_ENABLED 0/' flix/flix.ino && make
     - name: Check c_cpp_properties.json
       run: tools/check_c_cpp_properties.py
 

.markdownlint.json

@@ -7,6 +7,7 @@
 	"MD024": false,
 	"MD033": false,
 	"MD034": false,
+	"MD040": false,
 	"MD059": false,
 	"MD044": {
 		"html_elements": false,

docs/usage.md

@@ -243,9 +243,37 @@ In this mode, the pilot inputs are ignored (except the mode switch, if configure
 
 If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
 
+## Wi-Fi configuration
+
+You can configure the Wi-Fi using parameters and console commands.
+
+The Wi-Fi mode is chosen using `WIFI_MODE` parameter in QGroundControl or in the console:
+
+* `0` — Wi-Fi is disabled.
+* `1` — Access Point mode (*AP*) — the drone creates a Wi-Fi network.
+* `2` — Client mode (*STA*) — the drone connects to an existing Wi-Fi network.
+* `3` — *ESP-NOW (not implemented yet)*.
+
+> [!WARNING]
+> Tests showed that Client mode may cause **additional delays** in remote control (due to retranslations), so it's generally not recommended.
+
+The SSID and password are configured using the `ap` and `sta` console commands:
+
+```
+ap <ssid> <password>
+sta <ssid> <password>
+```
+
+Example of configuring the Access Point mode:
+
+```
+ap my-flix-ssid mypassword123
+p WIFI_MODE 1
+```
+
 ## Flight log
 
-After the flight, you can download the flight log for analysis wirelessly. Use the following for that:
+After the flight, you can download the flight log for analysis wirelessly. Use the following command on your computer for that:
 
 ```bash
 make log

flix/cli.ino

@@ -38,6 +38,8 @@ const char* motd =
 "raw/stab/acro/auto - set mode\n"
 "rc - show RC data\n"
 "wifi - show Wi-Fi info\n"
+"ap <ssid> <password> - setup Wi-Fi access point\n"
+"sta <ssid> <password> - setup Wi-Fi client mode\n"
 "mot - show motor output\n"
 "log [dump] - print log header [and data]\n"
 "cr - calibrate RC\n"
@@ -54,9 +56,7 @@ void print(const char* format, ...) {
 	vsnprintf(buf, sizeof(buf), format, args);
 	va_end(args);
 	Serial.print(buf);
-#if WIFI_ENABLED
 	mavlinkPrint(buf);
-#endif
 }
 
 void pause(float duration) {
@@ -64,9 +64,7 @@ void pause(float duration) {
 	while (t - start < duration) {
 		step();
 		handleInput();
-#if WIFI_ENABLED
 		processMavlink();
-#endif
 		delay(50);
 	}
 }
@@ -136,9 +134,11 @@ void doCommand(String str, bool echo = false) {
 		print("mode: %s\n", getModeName());
 		print("armed: %d\n", armed);
 	} else if (command == "wifi") {
-#if WIFI_ENABLED
 		printWiFiInfo();
-#endif
+	} else if (command == "ap") {
+		configWiFi(true, arg0.c_str(), arg1.c_str());
+	} else if (command == "sta") {
+		configWiFi(false, arg0.c_str(), arg1.c_str());
 	} else if (command == "mot") {
 		print("front-right %g front-left %g rear-right %g rear-left %g\n",
 			motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);

flix/flix.ino

@@ -7,7 +7,6 @@
 #include "quaternion.h"
 #include "util.h"
 
-#define WIFI_ENABLED 1
 
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -25,9 +24,7 @@ void setup() {
 	setupLED();
 	setupMotors();
 	setLED(true);
-#if WIFI_ENABLED
 	setupWiFi();
-#endif
 	setupIMU();
 	setupRC();
 	setLED(false);
@@ -42,9 +39,7 @@ void loop() {
 	control();
 	sendMotors();
 	handleInput();
-#if WIFI_ENABLED
 	processMavlink();
-#endif
 	logData();
 	syncParameters();
 }

flix/mavlink.ino

@@ -3,19 +3,16 @@
 
 // MAVLink communication
 
-#if WIFI_ENABLED
-
 #include <MAVLink.h>
 #include "util.h"
 
-#define SYSTEM_ID 1
-#define MAVLINK_RATE_SLOW 1
-#define MAVLINK_RATE_FAST 10
-
 extern float controlTime;
 
 bool mavlinkConnected = false;
 String mavlinkPrintBuffer;
+int mavlinkSysId = 1;
+Rate telemetryFast(10);
+Rate telemetrySlow(2);
 
 void processMavlink() {
 	sendMavlink();
@@ -28,10 +25,8 @@ void sendMavlink() {
 	mavlink_message_t msg;
 	uint32_t time = t * 1000;
 
-	static Rate slow(MAVLINK_RATE_SLOW), fast(MAVLINK_RATE_FAST);
-
-	if (slow) {
-		mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
+	if (telemetrySlow) {
+		mavlink_msg_heartbeat_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
 			(armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) |
 			((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
 			((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
@@ -40,27 +35,27 @@ void sendMavlink() {
 
 		if (!mavlinkConnected) return; // send only heartbeat until connected
 
-		mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+		mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
 			MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
 		sendMessage(&msg);
 	}
 
-	if (fast && mavlinkConnected) {
+	if (telemetryFast && mavlinkConnected) {
 		const float zeroQuat[] = {0, 0, 0, 0};
-		mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+		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, zeroQuat); // convert to frd
 		sendMessage(&msg);
 
-		mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
+		mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
 			channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
 		if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
 
 		float controls[8];
 		memcpy(controls, motors, sizeof(motors));
-		mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
+		mavlink_msg_actuator_control_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
 		sendMessage(&msg);
 
-		mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
+		mavlink_msg_scaled_imu_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time,
 			acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd
 			gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
 			0, 0, 0, 0);
@@ -95,7 +90,7 @@ void handleMavlink(const void *_msg) {
 	if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
 		mavlink_manual_control_t m;
 		mavlink_msg_manual_control_decode(&msg, &m);
-		if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast
+		if (m.target && m.target != mavlinkSysId) return; // 0 is broadcast
 
 		controlThrottle = m.z / 1000.0f;
 		controlPitch = m.x / 1000.0f;
@@ -108,11 +103,11 @@ void handleMavlink(const void *_msg) {
 	if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
 		mavlink_param_request_list_t m;
 		mavlink_msg_param_request_list_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 
 		mavlink_message_t msg;
 		for (int i = 0; i < parametersCount(); i++) {
-			mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+			mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
 				getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
 			sendMessage(&msg);
 		}
@@ -121,7 +116,7 @@ void handleMavlink(const void *_msg) {
 	if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
 		mavlink_param_request_read_t m;
 		mavlink_msg_param_request_read_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 
 		char name[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
 		strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
@@ -130,7 +125,7 @@ void handleMavlink(const void *_msg) {
 			memcpy(name, getParameterName(m.param_index), 16);
 		}
 		mavlink_message_t msg;
-		mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+		mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
 			name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);
 		sendMessage(&msg);
 	}
@@ -138,14 +133,15 @@ void handleMavlink(const void *_msg) {
 	if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {
 		mavlink_param_set_t m;
 		mavlink_msg_param_set_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 
 		char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
 		strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
-		setParameter(name, m.param_value);
+		bool success = setParameter(name, m.param_value);
+		if (!success) return;
 		// send ack
 		mavlink_message_t msg;
-		mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+		mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
 			m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
 		sendMessage(&msg);
 	}
@@ -153,17 +149,17 @@ void handleMavlink(const void *_msg) {
 	if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
 		mavlink_mission_request_list_t m;
 		mavlink_msg_mission_request_list_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 
 		mavlink_message_t msg;
-		mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
+		mavlink_msg_mission_count_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
 		sendMessage(&msg);
 	}
 
 	if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
 		mavlink_serial_control_t m;
 		mavlink_msg_serial_control_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 
 		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
 		strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated
@@ -175,7 +171,7 @@ void handleMavlink(const void *_msg) {
 
 		mavlink_set_attitude_target_t m;
 		mavlink_msg_set_attitude_target_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 
 		// copy attitude, rates and thrust targets
 		ratesTarget.x = m.body_roll_rate;
@@ -197,7 +193,7 @@ void handleMavlink(const void *_msg) {
 
 		mavlink_set_actuator_control_target_t m;
 		mavlink_msg_set_actuator_control_target_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 
 		attitudeTarget.invalidate();
 		ratesTarget.invalidate();
@@ -209,12 +205,12 @@ void handleMavlink(const void *_msg) {
 	if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
 		mavlink_log_request_data_t m;
 		mavlink_msg_log_request_data_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 
 		// Send all log records
 		for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
 			mavlink_message_t msg;
-			mavlink_msg_log_data_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
+			mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
 				sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
 			sendMessage(&msg);
 		}
@@ -224,13 +220,13 @@ void handleMavlink(const void *_msg) {
 	if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
 		mavlink_command_long_t m;
 		mavlink_msg_command_long_decode(&msg, &m);
-		if (m.target_system && m.target_system != SYSTEM_ID) return;
+		if (m.target_system && m.target_system != mavlinkSysId) return;
 		mavlink_message_t response;
 		bool accepted = false;
 
 		if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
 			accepted = true;
-			mavlink_msg_autopilot_version_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &response,
+			mavlink_msg_autopilot_version_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
 				MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
 			sendMessage(&response);
 		}
@@ -249,7 +245,7 @@ void handleMavlink(const void *_msg) {
 
 		// send command ack
 		mavlink_message_t ack;
-		mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
+		mavlink_msg_command_ack_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
 		sendMessage(&ack);
 	}
 }
@@ -266,7 +262,7 @@ void sendMavlinkPrint() {
 		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
 		strlcpy(data, str + i, sizeof(data));
 		mavlink_message_t msg;
-		mavlink_msg_serial_control_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+		mavlink_msg_serial_control_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
 			SERIAL_CONTROL_DEV_SHELL,
 			i + MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN < strlen(str) ? SERIAL_CONTROL_FLAG_MULTI : 0, // more chunks to go
 			0, 0, strlen(data), (uint8_t *)data, 0, 0);
@@ -274,5 +270,3 @@ void sendMavlinkPrint() {
 	}
 	mavlinkPrintBuffer.clear();
 }
-
-#endif

flix/parameters.ino

@@ -9,13 +9,19 @@
 extern float channelZero[16];
 extern float channelMax[16];
 extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern int wifiMode, udpLocalPort, udpRemotePort;
 
 Preferences storage;
 
 struct Parameter {
 	const char *name; // max length is 15 (Preferences key limit)
-	float *variable;
-	float value; // cache
+	bool integer;
+	union { float *f; int *i; }; // pointer to variable
+	float cache; // what's stored in flash
+	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; };
 };
 
 Parameter parameters[] = {
@@ -77,6 +83,14 @@ Parameter parameters[] = {
 	{"RC_THROTTLE", &throttleChannel},
 	{"RC_YAW", &yawChannel},
 	{"RC_MODE", &modeChannel},
+	// wifi
+	{"WIFI_MODE", &wifiMode},
+	{"WIFI_LOC_PORT", &udpLocalPort},
+	{"WIFI_REM_PORT", &udpRemotePort},
+	// mavlink
+	{"MAV_SYS_ID", &mavlinkSysId},
+	{"MAV_RATE_SLOW", &telemetrySlow.rate},
+	{"MAV_RATE_FAST", &telemetryFast.rate},
 };
 
 void setupParameters() {
@@ -84,10 +98,10 @@ void setupParameters() {
 	// Read parameters from storage
 	for (auto &parameter : parameters) {
 		if (!storage.isKey(parameter.name)) {
-			storage.putFloat(parameter.name, *parameter.variable);
+			storage.putFloat(parameter.name, parameter.getValue()); // store default value
 		}
-		*parameter.variable = storage.getFloat(parameter.name, *parameter.variable);
-		parameter.value = *parameter.variable;
+		parameter.setValue(storage.getFloat(parameter.name, 0));
+		parameter.cache = parameter.getValue();
 	}
 }
 
@@ -102,13 +116,13 @@ const char *getParameterName(int index) {
 
 float getParameter(int index) {
 	if (index < 0 || index >= parametersCount()) return NAN;
-	return *parameters[index].variable;
+	return parameters[index].getValue();
 }
 
 float getParameter(const char *name) {
 	for (auto &parameter : parameters) {
 		if (strcmp(parameter.name, name) == 0) {
-			return *parameter.variable;
+			return parameter.getValue();
 		}
 	}
 	return NAN;
@@ -117,7 +131,8 @@ float getParameter(const char *name) {
 bool setParameter(const char *name, const float value) {
 	for (auto &parameter : parameters) {
 		if (strcmp(parameter.name, name) == 0) {
-			*parameter.variable = value;
+			if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
+			parameter.setValue(value);
 			return true;
 		}
 	}
@@ -130,16 +145,18 @@ void syncParameters() {
 	if (motorsActive()) return; // don't use flash while flying, it may cause a delay
 
 	for (auto &parameter : parameters) {
-		if (parameter.value == *parameter.variable) continue;
-		if (isnan(parameter.value) && isnan(*parameter.variable)) continue; // handle NAN != NAN
-		storage.putFloat(parameter.name, *parameter.variable);
-		parameter.value = *parameter.variable;
+		if (parameter.getValue() == parameter.cache) continue; // no change
+		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
 	}
 }
 
 void printParameters() {
 	for (auto &parameter : parameters) {
-		print("%s = %g\n", parameter.name, *parameter.variable);
+		print("%s = %g\n", parameter.name, parameter.getValue());
 	}
 }
 

flix/wifi.ino

@@ -1,49 +1,76 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Wi-Fi support
-
-#if WIFI_ENABLED
+// Wi-Fi connectivity
 
 #include <WiFi.h>
 #include <WiFiAP.h>
 #include <WiFiUdp.h>
+#include "Preferences.h"
 
-#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"
+extern Preferences storage; // use the main preferences storage
+
+const int W_DISABLED = 0, W_AP = 1, W_STA = 2;
+int wifiMode = W_AP;
+int udpLocalPort = 14550;
+int udpRemotePort = 14550;
+IPAddress udpRemoteIP = "255.255.255.255";
 
 WiFiUDP udp;
 
 void setupWiFi() {
 	print("Setup Wi-Fi\n");
-	WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
-	udp.begin(WIFI_UDP_PORT);
+	if (wifiMode == W_AP) {
+		WiFi.softAP(storage.getString("WIFI_AP_SSID", "flix").c_str(), storage.getString("WIFI_AP_PASS", "flixwifi").c_str());
+	} else if (wifiMode == W_STA) {
+		WiFi.begin(storage.getString("WIFI_STA_SSID", "").c_str(), storage.getString("WIFI_STA_PASS", "").c_str());
+	}
+	udp.begin(udpLocalPort);
 }
 
 void sendWiFi(const uint8_t *buf, int len) {
-	if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return;
-	udp.beginPacket(udp.remoteIP() ? udp.remoteIP() : WIFI_UDP_REMOTE_ADDR, WIFI_UDP_REMOTE_PORT);
+	if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return;
+	udp.beginPacket(udpRemoteIP, udpRemotePort);
 	udp.write(buf, len);
 	udp.endPacket();
 }
 
 int receiveWiFi(uint8_t *buf, int len) {
 	udp.parsePacket();
+	if (udp.remoteIP()) udpRemoteIP = udp.remoteIP();
 	return udp.read(buf, len);
 }
 
 void printWiFiInfo() {
-	print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
-	print("SSID: %s\n", WiFi.softAPSSID().c_str());
-	print("Password: %s\n", WIFI_PASSWORD);
-	print("Clients: %d\n", WiFi.softAPgetStationNum());
-	print("Status: %d\n", WiFi.status());
-	print("IP: %s\n", WiFi.softAPIP().toString().c_str());
-	print("Remote IP: %s\n", udp.remoteIP().toString().c_str());
+	if (WiFi.getMode() == WIFI_MODE_AP) {
+		print("Mode: Access Point (AP)\n");
+		print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
+		print("SSID: %s\n", WiFi.softAPSSID().c_str());
+		print("Password: ***\n");
+		print("Clients: %d\n", WiFi.softAPgetStationNum());
+		print("IP: %s\n", WiFi.softAPIP().toString().c_str());
+	} else if (WiFi.getMode() == WIFI_MODE_STA) {
+		print("Mode: Client (STA)\n");
+		print("Connected: %d\n", WiFi.isConnected());
+		print("MAC: %s\n", WiFi.macAddress().c_str());
+		print("SSID: %s\n", WiFi.SSID().c_str());
+		print("Password: ***\n");
+		print("IP: %s\n", WiFi.localIP().toString().c_str());
+	} else {
+		print("Mode: Disabled\n");
+		return;
+	}
+	print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
 	print("MAVLink connected: %d\n", mavlinkConnected);
 }
 
-#endif
+void configWiFi(bool ap, const char *ssid, const char *password) {
+	if (ap) {
+		storage.putString("WIFI_AP_SSID", ssid);
+		storage.putString("WIFI_AP_PASS", password);
+	} else {
+		storage.putString("WIFI_STA_SSID", ssid);
+		storage.putString("WIFI_STA_PASS", password);
+	}
+	print("✓ Reboot to apply new settings\n");
+}

gazebo/flix.h

@@ -10,8 +10,6 @@
 #include "Arduino.h"
 #include "wifi.h"
 
-#define WIFI_ENABLED 1
-
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
 extern Vector rates;
@@ -73,3 +71,4 @@ void calibrateAccel() { print("Skip accel calibrating\n"); };
 void printIMUCalibration() { print("cal: N/A\n"); };
 void printIMUInfo() {};
 void printWiFiInfo() {};
+void configWiFi(bool, const char*, const char*) { print("Skip WiFi config\n"); };