Fix

flix/cli.ino

@@ -32,14 +32,15 @@ const char* motd =
 "ps - show pitch/roll/yaw\n"
 "psq - show attitude quaternion\n"
 "imu - show IMU data\n"
-"arm - arm the drone (when no mode switch)\n"
-"disarm - disarm the drone (when no mode switch)\n"
+"arm - arm the drone (when no armed switch)\n"
+"disarm - disarm the drone (when no armed switch)\n"
 "rc - show RC data\n"
 "mot - show motor output\n"
 "log - dump in-RAM log\n"
 "cr - calibrate RC\n"
 "ca - calibrate accel\n"
 "mfr, mfl, mrr, mrl - test motor (remove props)\n"
+"wifi - show Wi-Fi info\n"
 "sys - show system info\n"
 "reset - reset drone's state\n"
 "reboot - reboot the drone\n";
@@ -124,6 +125,7 @@ void doCommand(String str, bool echo = false) {
 		print("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
 			controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
 		print("mode: %s\n", getModeName());
+		print("armed: %d\n", armed);
 	} 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]);
@@ -141,6 +143,10 @@ void doCommand(String str, bool echo = false) {
 		testMotor(MOTOR_REAR_RIGHT);
 	} else if (command == "mrl") {
 		testMotor(MOTOR_REAR_LEFT);
+	} else if (command == "wifi") {
+#if WIFI_ENABLED
+		printWiFiInfo();
+#endif
 	} else if (command == "sys") {
 #ifdef ESP32
 		print("Chip: %s\n", ESP.getChipModel());

flix/failsafe.ino

@@ -29,7 +29,6 @@ void autoFailsafe() {
 
 	if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
 		if (mode == AUTO && !isfinite(controlMode)) {
-			print("Failsafe: regain control to pilot\n");
 			mode = STAB; // regain control to the pilot
 		}
 	}

flix/rc.ino

@@ -42,7 +42,7 @@ void normalizeRC() {
 	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
 	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
 	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
-	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default
+	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : NAN;
 	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
 }
 

flix/util.h

@@ -19,6 +19,14 @@ 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) {
+	return !isfinite(x);
+}
+
+bool valid(float x) {
+	return isfinite(x);
+}
+
 // Wrap angle to [-PI, PI)
 float wrapAngle(float angle) {
 	angle = fmodf(angle, 2 * PI);

flix/wifi.ino

@@ -13,6 +13,7 @@
 #define WIFI_PASSWORD "flixwifi"
 #define WIFI_UDP_PORT 14550
 #define WIFI_UDP_REMOTE_PORT 14550
+#define WIFI_UDP_ALWAYS_BROADCAST 1
 
 WiFiUDP udp;
 
@@ -24,7 +25,9 @@ void setupWiFi() {
 
 void sendWiFi(const uint8_t *buf, int len) {
 	if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return;
-	udp.beginPacket(WiFi.softAPBroadcastIP(), WIFI_UDP_REMOTE_PORT);
+	IPAddress remote = WiFi.softAPBroadcastIP();
+	if (!WIFI_UDP_ALWAYS_BROADCAST && udp.remoteIP()) remote = udp.remoteIP();
+	udp.beginPacket(remote, WIFI_UDP_REMOTE_PORT);
 	udp.write(buf, len);
 	udp.endPacket();
 }
@@ -34,4 +37,13 @@ int receiveWiFi(uint8_t *buf, int len) {
 	return udp.read(buf, len);
 }
 
+void printWiFiInfo() {
+	print("SSID: %s\n", WiFi.softAPSSID().c_str());
+	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());
+	print("Broadcast IP: %s\n", WiFi.softAPBroadcastIP().toString().c_str());
+}
+
 #endif

gazebo/flix.h

@@ -72,4 +72,5 @@ void calibrateGyro() { print("Skip gyro calibrating\n"); };
 void calibrateAccel() { print("Skip accel calibrating\n"); };
 void printIMUCalibration() { print("cal: N/A\n"); };
 void printIMUInfo() {};
+void printWiFiInfo() {};
 Vector accBias, gyroBias, accScale(1, 1, 1);