diff --git a/README.md b/README.md index 2b1ec93..64b4cd9 100644 --- a/README.md +++ b/README.md @@ -53,7 +53,7 @@ See [instructions on running the simulation](docs/build.md). |Type|Part|Image|Quantity| |-|-|:-:|:-:| |Microcontroller board|ESP32 Mini||1| -|IMU and barometer² board|GY-91 (or other MPU-9250/MPU-6500 board)||1| +|IMU (and barometer²) board|GY‑91 (or other MPU‑9250/MPU‑6500 board), ICM‑20948³||1| |Motor|8520 3.7V brushed motor (**shaft 0.8mm!**)||4| |Propeller|Hubsan 55 mm||4| |MOSFET (transistor)|100N03A or [analog](https://t.me/opensourcequadcopter/33)||4| @@ -65,13 +65,14 @@ See [instructions on running the simulation](docs/build.md). |Frame bottom part|3D printed:
[`flix-frame.stl`](docs/assets/flix-frame.stl) [`flix-frame.step`](docs/assets/flix-frame.step)||1| |Frame top part|3D printed:
[`esp32-holder.stl`](docs/assets/esp32-holder.stl) [`esp32-holder.step`](docs/assets/esp32-holder.step)||1| |Washer for IMU board mounting|3D printed:
[`washer-m3.stl`](docs/assets/washer-m3.stl) [`washer-m3.step`](docs/assets/washer-m3.step)||1| -|*RC transmitter (optional)*|*KINGKONG TINY X8 or other³*||1| -|*RC receiver (optional)*|*DF500 or other³*||1| +|*RC transmitter (optional)*|*KINGKONG TINY X8 or other⁴*||1| +|*RC receiver (optional)*|*DF500 or other⁴*||1| |Wires|28 AWG recommended||| |Tape, double-sided tape|||| *² — barometer is not used for now.*
-*³ — you may use any transmitter-receiver pair with SBUS interface.* +*³ — change `MPU9250` to `ICM20948` in `imu.ino` file if using ICM-20948 board.*
+*⁴ — you may use any transmitter-receiver pair with SBUS interface.* Tools required for assembly: @@ -127,19 +128,19 @@ Complete diagram is Work-in-Progress. |-|-| |GND|GND| |VIN|VC (or 3.3V depending on the receiver)| - |Signal|GPIO4⁴| + |Signal|GPIO4⁵| -*⁴ — UART2 RX pin was [changed](https://docs.espressif.com/projects/arduino-esp32/en/latest/migration_guides/2.x_to_3.0.html#id14) to GPIO4 in Arduino ESP32 core 3.0.* +*⁵ — UART2 RX pin was [changed](https://docs.espressif.com/projects/arduino-esp32/en/latest/migration_guides/2.x_to_3.0.html#id14) to GPIO4 in Arduino ESP32 core 3.0.* ### IMU placement -Required IMU orientation on the drone is **FLU** (Forward, Left, Up)⁵: +Required IMU orientation on the drone is **FLU** (Forward, Left, Up)⁶: GY-91 axis In case of using **FRD** orientation (Forward, Right, Down), use [the code for rotation](https://gist.github.com/okalachev/713db47e31bce643dbbc9539d166ce98). -*⁵ — This X/Y/Z IMU axis orientation is used in the Flix IMU library, internal accel/gyro/mag axes differ.* +*⁶ — This X/Y/Z IMU axis orientation is used in the Flix IMU library, internal accel/gyro/mag axes differ.* ## Version 0 diff --git a/docs/img/icm-20948.jpg b/docs/img/icm-20948.jpg new file mode 100644 index 0000000..5e6af23 Binary files /dev/null and b/docs/img/icm-20948.jpg differ diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md index 756bf71..e39daf3 100644 --- a/docs/troubleshooting.md +++ b/docs/troubleshooting.md @@ -13,19 +13,20 @@ 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. +* **Make sure correct IMU model is chosen**. If using ICM-20948 board, change `MPU9250` to `ICM20948` everywhere 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. -* **Check the motors**. Perform the following commands using Serial Monitor: - * `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). -* **Calibrate the RC** if you use it. Perform `rc` command and put the results to `rc.ino` file. -* **Check the RC data** if you use it. Use `rc` command, `Control` should show correct values between -1 and 1, and between 0 and 1 for the throttle. * **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**. * **Make sure you're not moving the drone several seconds after the power on**. The drone calibrates its gyroscope on the start so it should stay still for a while. * **Check the IMU sample rate**. Perform `imu` command. The `rate` field should be about 1000 (Hz). * **Check the IMU data**. Perform `imu` command, check raw accelerometer and gyro output. The output should change as you move the drone. * **Calibrate the accelerometer.** if is wasn't done before. Perform `ca` command and put the results to `imu.ino` file. * **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. +* * **Check the motors**. Perform the following commands using Serial Monitor: + * `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). +* **Calibrate the RC** if you use it. Perform `rc` command and put the results to `rc.ino` file. +* **Check the RC data** if you use it. Use `rc` command, `Control` should show correct values between -1 and 1, and between 0 and 1 for the throttle. * **Check the IMU output using QGroundControl**. Connect to the drone using QGroundControl on your computer. Go to the *Analyze* tab, *MAVLINK Inspector*. Plot the data from the `SCALED_IMU` message. The gyroscope and accelerometer data should change according to the drone movement. * **Check the gyroscope only attitude estimation**. Comment out `applyAcc();` line in `estimate.ino` and check if the attitude estimation in QGroundControl. It should be stable, but only drift very slowly. diff --git a/flix/imu.ino b/flix/imu.ino index abc76a5..9f29767 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -34,8 +34,8 @@ void setupIMU() { void configureIMU() { IMU.setAccelRange(IMU.ACCEL_RANGE_4G); IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS); - IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ); - IMU.setSrd(0); // set sample rate to 1000 Hz + IMU.setDLPF(IMU.DLPF_MAX); + IMU.setRate(IMU.RATE_1KHZ_APPROX); } void readIMU() { @@ -67,8 +67,6 @@ void calibrateGyro() { void calibrateAccel() { Serial.println("Calibrating accelerometer"); IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode - IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_20HZ); - IMU.setSrd(19); Serial.setTimeout(60000); Serial.print("Place level [enter] "); Serial.readStringUntil('\n'); @@ -89,7 +87,7 @@ void calibrateAccel() { } void calibrateAccelOnce() { - const int samples = 100; + const int samples = 1000; static Vector accMax(-INFINITY, -INFINITY, -INFINITY); static Vector accMin(INFINITY, INFINITY, INFINITY); @@ -125,6 +123,6 @@ void printIMUCal() { } void printIMUInfo() { - Serial.printf("type: %s\n", IMU.getType()); + Serial.printf("model: %s\n", IMU.getModel()); Serial.printf("who am I: 0x%02X\n", IMU.whoAmI()); }