Disarm the drone on simulator plugin reset

In order to reset yaw target.

README.md

@@ -87,7 +87,7 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
 |Tape, double-sided tape||||
 
 *² — barometer is not used for now.*<br>
-*³ — change `MPU9250` to `ICM20948` in `imu.ino` file if using ICM-20948 board.*<br>
+*³ — change `MPU9250` to `ICM20948` or `MPU6050` in `imu.ino` file for using the appropriate boards.*<br>
 *³⁻¹ — MPU-6050 supports I²C interface only (not recommended). To use it change IMU declaration to `MPU6050 IMU(Wire)`.*<br>
 *⁴ — this frame is optimized for GY-91 board, if using other, the board mount holes positions should be modified.*<br>
 *⁵ — you also may use any transmitter-receiver pair with SBUS interface.*

docs/troubleshooting.md

@@ -15,7 +15,7 @@ Do the following:
 * **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
 * **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
 * **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200.
-* **Make sure correct IMU model is chosen**. If using ICM-20948 board, change `MPU9250` to `ICM20948` everywhere in the `imu.ino` file.
+* **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
 * **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
 * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
 * **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.

flix/control.ino

@@ -80,7 +80,7 @@ void interpretControls() {
 
 	if (mode == STAB) {
 		float yawTarget = attitudeTarget.getYaw();
-		if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or pilot commands yaw rate
+		if (!armed || invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target
 		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
 		ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU
 	}

flix/imu.ino

@@ -4,7 +4,7 @@
 // Work with the IMU sensor
 
 #include <SPI.h>
-#include <MPU9250.h>
+#include <FlixPeriph.h>
 #include "lpf.h"
 #include "util.h"
 
@@ -125,6 +125,7 @@ void printIMUInfo() {
 	print("rate: %.0f\n", loopRate);
 	print("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
 	print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
+	IMU.waitForData();
 	Vector rawGyro, rawAcc;
 	IMU.getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
 	IMU.getAccel(rawAcc.x, rawAcc.y, rawAcc.z);

flix/mavlink.ino

@@ -12,6 +12,7 @@
 #define PERIOD_FAST 0.1
 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
 
+bool mavlinkConnected = false;
 String mavlinkPrintBuffer;
 
 extern double controlTime;
@@ -41,12 +42,14 @@ void sendMavlink() {
 			mode, MAV_STATE_STANDBY);
 		sendMessage(&msg);
 
+		if (!mavlinkConnected) return; // send only heartbeat until connected
+
 		mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
 			MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
 		sendMessage(&msg);
 	}
 
-	if (t - lastFast >= PERIOD_FAST) {
+	if (t - lastFast >= PERIOD_FAST && mavlinkConnected) {
 		lastFast = t;
 
 		const float zeroQuat[] = {0, 0, 0, 0};
@@ -80,6 +83,7 @@ void sendMessage(const void *msg) {
 void receiveMavlink() {
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 	int len = receiveWiFi(buf, MAVLINK_MAX_PACKET_LEN);
+	if (len) mavlinkConnected = true;
 
 	// New packet, parse it
 	mavlink_message_t msg;

flix/wifi.ino

@@ -9,8 +9,11 @@
 #include <WiFiAP.h>
 #include <WiFiUdp.h>
 
-#define WIFI_SSID "flix"
-#define WIFI_PASSWORD "flixwifi"
+#define WIFI_AP_MODE 1
+#define WIFI_AP_SSID "flix"
+#define WIFI_AP_PASSWORD "flixwifi"
+#define WIFI_SSID ""
+#define WIFI_PASSWORD ""
 #define WIFI_UDP_PORT 14550
 #define WIFI_UDP_REMOTE_PORT 14550
 #define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
@@ -19,7 +22,11 @@ WiFiUDP udp;
 
 void setupWiFi() {
 	print("Setup Wi-Fi\n");
-	WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
+	if (WIFI_AP_MODE) {
+		WiFi.softAP(WIFI_AP_SSID, WIFI_AP_PASSWORD);
+	} else {
+		WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
+	}
 	udp.begin(WIFI_UDP_PORT);
 }
 

gazebo/simulator.cpp

@@ -59,6 +59,7 @@ public:
 
 	void OnReset() {
 		attitude = Quaternion(); // reset estimated attitude
+		armed = false;
 		__resetTime += __micros;
 		gzmsg << "Flix plugin reset" << endl;
 	}