Add support form arm/disarm mavlink command

flix/cli.ino

@@ -8,12 +8,10 @@
 #include "util.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern const int ACRO, STAB, AUTO;
 extern float loopRate, dt;
 extern double t;
 extern uint16_t channels[16];
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
-extern int mode;
 extern bool armed;
 
 const char* motd =
@@ -36,13 +34,13 @@ const char* motd =
 "imu - show IMU data\n"
 "arm - arm the drone (when no armed switch)\n"
 "disarm - disarm the drone (when no armed switch)\n"
-"stab/acro/auto - set mode (when no mode 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";
@@ -119,12 +117,6 @@ void doCommand(String str, bool echo = false) {
 		armed = true;
 	} else if (command == "disarm") {
 		armed = false;
-	} else if (command == "stab") {
-		mode = STAB;
-	} else if (command == "acro") {
-		mode = ACRO;
-	} else if (command == "auto") {
-		mode = AUTO;
 	} else if (command == "rc") {
 		print("channels: ");
 		for (int i = 0; i < 16; i++) {
@@ -151,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/control.ino

@@ -34,8 +34,7 @@
 #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
-int mode = STAB;
+enum { MANUAL, ACRO, STAB, AUTO } mode = STAB;
 bool armed = false;
 
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);

flix/mavlink.ino

@@ -226,8 +226,8 @@ void handleMavlink(const void *_msg) {
 		}
 
 		if (m.command == MAV_CMD_DO_SET_MODE) {
-			if (!(m.param2 >= 0 && m.param2 <= AUTO)) return; // incorrect mode
-			mode = m.param2;
+			if (!(m.param2 >= 0 && m.param2 <= AUTO)) return;
+			mode = static_cast<decltype(mode)>(m.param2);
 			mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg.sysid, msg.compid);
 			sendMessage(&ack);
 		}

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);

tools/pyflix/flix.py

@@ -173,10 +173,6 @@ class Flix:
             self.motors = msg.controls[:4]  # type: ignore
             self._trigger('motors', self.motors)
 
-        # TODO: to be removed: the old way of passing motor outputs
-        if isinstance(msg, mavlink.MAVLink_actuator_output_status_message):
-            self.motors = msg.actuator[:4]  # type: ignore
-
         if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
             self.acc = self._mavlink_to_flu([msg.xacc / 1000, msg.yacc / 1000, msg.zacc / 1000])
             self.gyro = self._mavlink_to_flu([msg.xgyro / 1000, msg.ygyro / 1000, msg.zgyro / 1000])