furyhawk/esp-drone
1
0
Fork 0
mirror of https://github.com/espressif/esp-drone.git synced 2026-07-21 10:15:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'docs/provide_translation_for_drivers_communication_and_system' into 'master'

Browse Source 
docs: provide English version documents and update several existing documents

See merge request ae_group/esp-college/esp-drone!29
This commit is contained in:
Wang Fang
2020-12-28 14:33:37 +08:00
parent 673175cc10 4869b0342b
commit c73a87908d
19 changed files with 3219 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/_static/mpu6050_dmp_en.png
Vendored
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 119 KiB

docs/en/rst/communication.rst
+222 -3
View File
@@ -1,3 +1,222 @@
================
Communications
================
Communication Protocols
========================
:link_to_translation:`zh_CN:[中文]`
Communication Hierarchy
--------------------------
================== =================== =======================
Terminal Mobile/PC ESP-Drone
Application Layer APP Flight Control Firmware
Protocol Layer CRTP CRTP
Transport Layer UDP UDP
Physical Layer Wi-Fi STA (Station) Wi-Fi AP (Access Point)
================== =================== =======================
Wi-Fi Communication
--------------------
Wi-Fi Performance
~~~~~~~~~~~~~~~~~~
**ESP32 Wi-Fi Performance**
===================== ================================================================================================
Item Parameter
===================== ================================================================================================
Mode STA mode, AP mode, STA+AP mode
Protocol IEEE 802.11b/g/n, and 802.11 LR (Espressif). Support switching over software
Security WPA, WPA2, WPA2-Enterprise, WPS
Main Feature AMPDU, HT40, QoS
Supported Distance 1 km under the Exclusive Agreement of Espressif
Transfer Rate 20 Mbit/s TCP throughput, 30 Mbit/s UDP
===================== ================================================================================================
For other parameters, see `ESP32 Wi-Fi Feature List <https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-guides/wifi.html#esp32-wi-fi-feature-list>`__\.
**ESP32-S2 Wi-Fi Performance**
===================== ============================================================
Item Parameter
===================== ============================================================
Mode STA mode, AP mode, STA+AP mode
Protocol IEEE 802.11b/g/n. Support switch over software
Security WPA, WPA2, WPA2-Enterprise, WPS
Main Feature AMPDU, HT40, QoS
Supported Distance 1 km under the Exclusive Agreement of Espressif
Transfer Rate 20 Mbit/s TCP throughput, 30 Mbit/s UDP
===================== ============================================================
For other parameters, see `ESP32-S2 Wi-Fi Feature List <https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/api-guides/wifi.html#esp32-s2-wi-fi-feature-list>`__\.
Wi-Fi Programming Framework
~~~~~~~~~~~~~~~~~~~~~~~~~~~~
**Wi-Fi Programming Framework Based on ESP-IDF**
.. figure:: https://img-blog.csdnimg.cn/20200423173923300.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70#pic_center
:align: center
:alt: Wi-Fi Programming Example
:figclass: align-center
Wi-Fi Programming Example
**General Programming Process**
1. At application layer, call `Wi-Fi Driver API <https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_wifi.html>`__ to initialize Wi-Fi.
2. Wi-Fi driver is transparent to developers. When an event occurs, Wi-Fi driver sends an ``event`` to `default event loop <https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/system/esp_event.html#esp-event-default-loops>`__. Applications can write and register ``handle`` program on demand.
3. Network interface component `esp_netif <https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_netif.html>`__ provides the ``handle`` program associated with the Wi-Fi driver ``event`` by default. For example, when ESP32 works as an AP, and a user connects to this AP, esp_netif will automatically start the DHCP service.
For the detailed usage, please refer to the code ``\components\drivers\general\wifi\wifi_esp32.c``\.
.. note::
Before Wi-Fi initialization, please use ``WIFI_INIT_CONFIG_DEFAULT`` to obtain the initialization configuration struct, and customize this struct first, then start the initialization. Be aware of problems caused by unitialized members of the struct, and pay special attention to this issue when new structure members are added to the ESP-IDF during update.
**AP Mode Workflow**
.. figure:: https://img-blog.csdnimg.cn/2020042622523887.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70#pic_center
:align: center
:alt: Sample Wi-Fi Event Scenarios in AP Mode
:figclass: align-center
Sample Wi-Fi Event Scenarios in AP Mode
Increase Wi-Fi Communication Distance
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Navigate to ``Component config>>PHY>>Max WiFi TX power (dBm)``, and update ``Max WiFi TX power`` to ``20``. This configuration increases the PHY gain and Wi-Fi communication distance.
UDP Communication
------------------
UDP Port
~~~~~~~~~~
===================== =================== =======================
App Direction ESP-Drone
===================== =================== =======================
192.168.43.42::2399 TX/RX 192.168.43.42::2390
===================== =================== =======================
UDP Packet Structure
~~~~~~~~~~~~~~~~~~~~
.. code:: text
/* Frame format:
* +=============+-----+-----+
* | CRTP | CKSUM |
* +=============+-----+-----+
*/
- The packet transmitted by the UDP: CRTP + verification information.
- CRTP: As defined by the CRTP packet structure, it contains Header and Data, as detailed in the CRTP protocol section.
- CKSUM: the verification information. Its size is 1 byte, and this CKSUM is incremented by CRTP packet byte.
**CKSUM Calculation Method**
.. code:: python
#take python as an example: Calculate the raw cksum and add it to the end of the packet
raw = (pk.header,) + pk.datat
cksum = 0
for i in raw:
cksum += i
cksum %= 256
raw = raw + (cksum,)
CRTP Protocol
------------------
The ESP-Drone project continues the CRTP protocol used by the Crazyflie project for flight instruction sending, flight data passback, parameter settings, etc.
CRTP implements a stateless design that does not require a handshake step. Any command can be sent at any time, but for some log/param/mem commands, the TOC (directory) needs to be downloaded to assist the host in sending the information correctly. The implemented Python API (cflib) can download param/log/mem TOC to ensure that all functions are available.
CRTP Packet Structure
~~~~~~~~~~~~~~~~~~~~~~
The 32-byte CRTP packet contains one byte of Header and 31 bytes of Payload. Header records the information about the ports (4 bits), channels (2 bits), and reserved bits (2 bits).
.. code:: text
7 6 5 4 3 2 1 0
+---+---+---+---+---+---+---+---+
| Port | Res. | Chan. |
+---+---+---+---+---+---+---+---+
| DATA 0 |
+---+---+---+---+---+---+---+---+
: : : : : : : : :
+---+---+---+---+---+---+---+---+
| DATA 30 |
+---+---+---+---+---+---+---+---+
======== ======== ============== =============================
Field Byte Bit Description
======== ======== ============== =============================
Header 0 0 ~ 1 Target data channel
\ 0 2 ~ 3 Reserved for transport layer
\ 0 4 ~ 7 Target data port
Data 1 ~ 31 0 ~ 7 The data in this packet
======== ======== ============== =============================
Port Allocation
~~~~~~~~~~~~~~~~
====== ===================== ===================================================================================
Port Target Purpose
====== ===================== ===================================================================================
0 Console Read console text that is printed to the console on the Crazyflie using consoleprintf.
2 Parameters Get/set parameters from the Crazyflie. Parameters are defined using a macro in the Crazyflie source-code
3 Commander Send control set-points for the roll/pitch/yaw/thrust regulators
4 Memory access Access non-volatile memories like 1-wire and I2C (only supported for Crazyflie 2.0)
5 Data logging Set up log blocks with variables that will be sent back to the Crazyflie at a specified period. Log variables are defined using a macro in the Crazyflie source-code
6 Localization Packets related to localization
7 Generic Setpoint Allows to send setpoint and control modes
13 Platform Used for misc platform control, like debugging and power off
14 Client-side debugging Debugging the UI and exists only in the Crazyflie Python API and not in the Crazyflie itself.
15 Link layer Used to control and query the communication link
====== ===================== ===================================================================================
Most of the modules in the firmware that are connected to the port are implemented as tasks. If an incoming CRTP packet is delivered in the messaging queue, the task is blocked in the queue. At startup, each task and other modules need to be registered for a predefined port at the communication link layer.
Details of the use of each port can be found at \ `CRTP - Communicate with Crazyflie <https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/>`__\.
Supported Package by CRTP Protocol
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
cflib is a Python package supported by CRTP protocol, and provides an application-layer interface for communication protocols that can be used to build an upper PC, to communicate with Crazyflie and Crazyflie 2.0 quadcopters.
Each component in the firmware that uses the CRTP protocol has a script corresponding to it in cflib.
- Source repository: `crazyflie-lib-python <https://github.com/bitcraze/crazyflie-lib-python>`__.
- cflib repository specially for ESP-Drone: `qljz1993/crazyflie-lib-python <https://github.com/qljz1993/crazyflie-lib-python.git>`__. Please checkout to ``esplane`` branch.
Application Development Based on CRTP Protocol
------------------------------------------------
Examples for Various Platform
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1. `crazyflie2-ios-client <https://github.com/bitcraze/crazyflie2-ios-client>`__
2. `crazyflie2-windows-uap-client <https://github.com/bitcraze/crazyflie2-windows-uap-client>`__
3. `crazyflie-android-client <https://github.com/bitcraze/crazyflie-android-client>`__
4. `User Guide on Android <https://wiki.bitcraze.io/doc:crazyflie:client:cfandroid:index>`__
5. `Development Guide on Android <https://wiki.bitcraze.io/doc:crazyflie:dev:env:android>`__
cfclient
~~~~~~~~
cfclient is the upper PC for ``Crazeflie`` project, which has fully implemented the functions defined in ``CRTP`` Protocol, and speeds up the debug process for the drone. The ESP-Drone project tailors and adjusts the upper PC to meet functional design needs.
.. figure:: ../../_static/cfclient.png
:align: center
:alt: Cfclient Control Interface
:figclass: align-center
Cfclient Control Interface
For detailed information about cfclient, please refer to `cfclient <gettingstarted.html#pc-cfclient>`__.
docs/en/rst/developerguide.rst
+8 -35
View File
@@ -1,39 +1,12 @@
Develop Guide
=================
:link_to_translation:`zh_CN:[中文]`
- Set up environment
.. toctree::
:maxdepth: 2
- Set up ESP-IDF environment
- Modify ESP32/ESP32-S2 link script
- Access to project source code
- Drivers
- General device
- I2C bus device
- SPI bus device
- Flight control system
- System boot process
- System task management
- Introductin to key tasks
- Sensor hardware abstraction
- Sensor calibration
- Attitude calculation
- Control algorithm
- Communication protocol
- Communication hierarchy
- Wi-Fi communication
- UDP communication
- CRTP protocol
- Application development based on CRTP protocol
- H/W Reference
- Supported hardware list
- ESP32_S2_Drone_V1_2
- ESPlane_V2_S2
- ESPlane FC V1
Set up Development Environment <getespidf>
Drivers <drivers>
Flight Control System <system>
Communication Protocols <communication>
Hardware Reference <hardware>
docs/en/rst/drivers.rst
+711 -2
View File
@@ -1,3 +1,712 @@
========
Drivers
========
========
:link_to_translation:`zh_CN:[中文]`
This section covers I2C drivers and SPI drivers used in ESP-Drone.
I2C Drivers
-------------
I2C drivers include MPU6050 sensor driver and VL53LXX sensor driver. The following part describes the two sensors in their main features, key registers and programming notes, etc.
MPU6050 Sensor
~~~~~~~~~~~~~~
Overview
^^^^^^^^^^^^^
The MPU6050 is a 6-axis Motion Tracking device that combines a 3-axis gyroscope, a 3-axis accelerometer, and a Digital Motion Processor (DMP).
How It Works
^^^^^^^^^^^^^
- Gyroscope: vibration occurs due to Coriolis effect when the gyroscope rotates around any sensing axis. Such vibration can be detected by a capacitive sensor. This capacitive sensor can amplify, demodulate, and filter such vibration signal, then generate a voltage proportional to the angular velocity.
- Accelerometer: when displacement accelerates along a specific axis over the corresponding detection mass, the capacitive sensor detects a change in capacitance.
Measure Range
^^^^^^^^^^^^^
- Gyroscope full-scale range: ±250 °/sec, ±500 °/sec, ±1000 °/sec, ±2000 °/sec
- Accelerometer full-scale range: ±2 g, ±4 g, ±8 g, ±16 g
AUX I2C Interface
^^^^^^^^^^^^^^^^^
- MPU6050 has an auxiliary I2C bus for communication with external 3-Axis magnetometer or other sensors.
- The AUX I2C interface supports two operation modes: I2C Master mode or Pass-Through mode.
MPU6050 FIFO
^^^^^^^^^^^^
The MPU6050 contains a 1024-byte FIFO register that is accessible via the Serial Interface. The FIFO configuration register determines which data is written into the FIFO. Possible choices include gyroscope data, accelerometer data, temperature readings, auxiliary sensor readings, and FSYNC input.
Digital Low-Pass Filter (DLPF)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
The MPU6050 has its own low-pass filter. Users can configure :ref:`MPU6050-Register-26` to control bandwidth, to lower high-frequency interference, but the configuration may reduce sensor input rate. Enable the DLPF: accelerometer outputs 1 kHz signal; disable the DLPF: accelerometer outputs 8 kHz signal.
Frame Synchronization Sampling Pin (FSYNC)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
The field EXT_SYNC_SET in Register 26 is used to configure the external Frame Synchronization (FSYNC) pin sampling.
Digital Motion Processing (DMP)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- The MPU6050 has its own DMP inside, which can be used to calculate quaternion to offload processing power from the main CPU.
- The DMP can trigger an interrupt via a pin.
.. figure:: ../../_static/mpu6050_dmp_en.png
:align: center
:alt: MPU6050 DMP
:figclass: align-center
MPU6050 DMP
MPU6050 Orientation
^^^^^^^^^^^^^^^^^^^^^^^
The MPU6050 defines its orientation as follows.
.. figure:: ../../_static/mpu6050_xyz.png
:align: center
:alt: mpu6050_xyz
:figclass: align-center
MPU6050 Axis of X, Y, Z
MPU6050 Initialization
^^^^^^^^^^^^^^^^^^^^^^^^
1. Restore register defaults:
- Set PWR_MGMT_1 bit7 to 1,
- and then to 0 after register defaults are restored.
- Bit6 is automatically set to 1, putting MPU6050 into sleep mode;
2. Set PWR_MGMT_1 bit6 to 0, to wake up the sensor;
3. Set the clock source;
4. Set the range for the gyroscope and accelerometer;
5. Set the sample rate;
6. Set Digital Low-Pass Filter (optional).
MPU6050 Key Registers
^^^^^^^^^^^^^^^^^^^^^^^
Typical Register Values
'''''''''''''''''''''''''
.. list-table::
:widths: 25 25 50
:header-rows: 1
* - Register
- Typical Value
- Feature
* - PWR_MGMT_1
- 0x00
- Normal Enable
* - SMPLRT_DIV
- 0x07
- Gyroscope sample rate: 125 Hz
* - CONFIG
- 0x06
- DLPF filter frequency: 5 Hz
* - GYRO_CONFIG
- 0x18
- Gyroscope does not conduct self-test, and its full-scale output is ±2000 °/s
* - ACCEL_CONFIG
- 0x01
- Accelerometer does not conduct self-test, and its full-scale output is ±2 g.
Register 117 WHO_AM_I - Device Address
'''''''''''''''''''''''''''''''''''''''
Bits [6:1] store the device address, and default to 0x68. The value of the AD0 pin is not reflected in this register.
.. figure:: ../../_static/REG_75.png
:align: center
:alt: WHO_AM_I
:figclass: align-center
Register 107 PWR_MGMT_1 - Power Management 1
''''''''''''''''''''''''''''''''''''''''''''
.. figure:: ../../_static/REG_6B.png
:align: center
:alt: PWR_MGMT_1
:figclass: align-center
- DEVICE_RESET: if this bit is set, the register will use the default value.
- SLEEP: if this bit is set, MPU6050 will be put into sleep mode.
- CYCLE: when this bit (CYCLE) is set to 1 while SLEEP is disabled, the MPU6050 will be put into Cycle mode. In Cycle mode, the MPU6050 cycles between sleep mode and waking up to take a single sample of data from active sensors at a rate determined by LP_WAKE_CTRL (Register 108).
.. _MPU6050-Register-26:
Register 26 CONFIG - Configure DLPF
'''''''''''''''''''''''''''''''''''''
.. figure:: ../../_static/REG_1A.png
:align: center
:alt: CONFIG
:figclass: align-center
The DLPF is configured by DLPF_CFG. The accelerometer and gyroscope are filtered according to the value of DLPF_CFG as shown in the table below.
.. figure:: ../../_static/DLPF_CFG.png
:align: center
:alt: DLPF
:figclass: align-center
Register 27 - GYRO_CONFIG - Configure Gyroscope’s Full-Scale Range
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
.. figure:: ../../_static/REG_1B.png
:align: center
:alt: GYRO_CONFIG
:figclass: align-center
- XG_ST: if this bit is set, X-axis gyroscope performs self-test.
- FS_SEL: select the full scale of the gyroscope, see the table below for details.
.. figure:: ../../_static/FS_SEL.png
:align: center
:alt: FS_SEL
:figclass: align-center
Register 28 ACCEL_CONFIG - Configure Accelerometer’s Full-Scale Range
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
Users can configure the full-scale range for the accelerometer according to the table below.
.. figure:: ../../_static/REG_1C.png
:align: center
:alt: ACCEL_CONFIG
:figclass: align-center
.. figure:: ../../_static/AFS_SEL.png
:align: center
:alt: AFS_SEL
:figclass: align-center
Register 25 SMPRT_DIV - Sample Rate Divider
'''''''''''''''''''''''''''''''''''''''''''''''
The register specifies the divider from the gyroscope output rate used to generate the Sample Rate for the MPU6050. The sensor register output, FIFO output, and DMP sampling are all based on the Sample Rate. The Sample Rate is generated by dividing the gyroscope output rate by (1 + SMPLRT_DIV).
.. figure:: ../../_static/REG_19.png
:align: center
:alt: SMPRT_DIV
:figclass: align-center
..
Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
Gyroscope Output Rate = 8 kHz when the DLPF is disabled (DLPF_CFG = 0 0r 7), and 1 kHz when the DLPF is enabled (see Register 26). Note: if SMPLRT_DIV is set to 7 when the DLPF is disabled, the chip can generate an interrupt signal of 1 kHz.
.. figure:: ../../_static/mpu6050_int_plot.png
:align: center
:alt: SMPLRT_DIV=7
:figclass: align-center
Registers 59 ~ 64 - Accelerometer Measurements
'''''''''''''''''''''''''''''''''''''''''''''''''
.. figure:: ../../_static/REG_3B_40.png
:align: center
:alt: REG_3B_40
:figclass: align-center
- Save data in big-endian: higher data bits are stored at low address, and lower bits at high address.
- Save data in complement: the measurement value is a signed integer, so it can be stored in complement.
Registers 65 ~ 66 - Temperature Measurement
'''''''''''''''''''''''''''''''''''''''''''''
.. figure:: ../../_static/REG_41_42.png
:align: center
:alt: REG_41_42
:figclass: align-center
Registers 67 ~ 72 - Gyroscope Measurements
''''''''''''''''''''''''''''''''''''''''''''
.. figure:: ../../_static/REG_43_48.png
:align: center
:alt: REG_43_48
:figclass: align-center
VL53LXX Sensor
~~~~~~~~~~~~~~
**Overview**
VL53L1X is a Time-of-Flight ranging and gesture detection sensor provided by ST.
**How It Works**
The VL53L0X/VL53L1X chip is internally integrated with a laser transmitter and a SPAD infrared receiver. By detecting the time difference between photon sending and receiving, the chip calculates the flight distance of photons, and the maximum measuring distance can reach two meters, which is suitable for short- and medium-range measurement applications.
.. figure:: ../../_static/vl53l1x_package.png
:align: center
:alt: VL53LXX
:figclass: align-center
VL53LXX
**Measurement Area - Region-of-Interest (ROI)**
The VL53L0X/VL53L1X measures the shortest distance in the measurement area, which can be zoomed in or out depending on actual scenario. But a large detection range may cause fluctuations in the measurement.
For more information about configuration on measurement area, see the sections 2.4 Ranging Description and 2.8 Sensing Array Optical Center in `VL53LXX Datasheet <https://www.st.com/resource/en/datasheet/vl53l1x.pdf>`_.
.. figure:: ../../_static/vl53lxx_roi.png
:align: center
:alt: ROI
:figclass: align-center
ROI
**Measure Distance**
- The VL53L0X sensor has a blind spot of **3 ~ 4 cm**, with an effective measurement range of 3 ~ 200 cm and an accuracy of ±3%.
- The VL53L1X is an upgraded version of the VL53L0X with a detection distance of up to 400 cm.
.. list-table::
:widths: 5 5 5 35
:header-rows: 1
* - Precision Mode
- Measuring Time (ms)
- Measuring Range (m)
- Applications
* - Default
- 30
- 1.2
- Standard
* - High Precision
- 200
- 1.2 <+-3%
- Precise distance measurement
* - Long Range
- 33
- 2
- Long distance, only works in dark environment
* - High Speed
- 20
- 1.2 +-5%
- Speed priority with low precision
- VL53LXX measurement distance/performance is related to the environment. The detection distance is farther in the dark conditions. But in outdoor bright conditions, the laser sensor may be subject to a lot of interference, resulting in reduced measurement accuracy. For such reason, the height should be set based on the outdoor air pressure.
.. figure:: ../../_static/vl53l1x_max_distance.png
:align: center
:alt: VL53L1X mode
:figclass: align-center
**Measurement Frequency**
- The VL53L0X ranging frequency is up to 50 Hz, with a measurement error of ±5%.
- The VL53L1X I2C has a maximum clock frequency of 400 kHz, and the pull-up resistor needs to be selected based on voltage and bus capacitance values. For more information, see `VL53LXX Datasheet <https://www.st.com/resource/en/datasheet/vl53l1x.pdf>`__\.
.. figure:: ../../_static/vl53l1x_typical_circuit.png
:align: center
:alt: vl53l1x
:figclass: align-center
VL53L1X
- XSHUT, input pin for mode selection (sleep), must always be driven to avoid leakage current. A pull-up resistor is needed.
- GPIO1, interrupt output pin for measuring dataready interrupts.
**Work Mode**
By setting the level of the XSHUT pin, the sensor can be switched into HW Standby mode or SW Standby mode for conditional boot and for lowering the standby power consumption. If the host gives up the management of sensor modes, the XSHUT pin can be set to pull up by default.
- HW Standby: XSHUT pulls down and the sensor power is off.
- SW Standby: XSHUT pulls up, then the sensor enters Boot and SW Standby modes.
.. figure:: ../../_static/vl53lxx_power_up_sequence.png
:align: center
:alt: HW Standby
:figclass: align-center
HW Standby
.. figure:: ../../_static/vl53lxx_boot_sequence.png
:align: center
:alt: SW Standby
:figclass: align-center
SW Standby
VL53LXX Initialization
^^^^^^^^^^^^^^^^^^^^^^^
1. Wait for the initialization of the hardware to complete
2. Data initialization
3. Static initialization, loading data
4. Set ranging mode
5. Set the maximum wait time for a single measurement
6. Set the measurement frequency (interval)
7. Set measurement area ROI (optional)
8. Start the measurement
.. code:: text
/*init vl53l1 module*/
void vl53l1_init()
{
Roi0.TopLeftX = 0; //Measurement target area (optional). Min: 4*4, Max: 16*16.
Roi0.TopLeftY = 15;
Roi0.BotRightX = 7;
Roi0.BotRightY = 0;
Roi1.TopLeftX = 8;
Roi1.TopLeftY = 15;
Roi1.BotRightX = 15;
Roi1.BotRightY = 0;
int status = VL53L1_WaitDeviceBooted(Dev); // Wait for the initialization of the hardware to complete
status = VL53L1_DataInit(Dev); // Data initialization, is executed immediately after wake up;
status = VL53L1_StaticInit(Dev); // Static initialization, loading parameters.
status = VL53L1_SetDistanceMode(Dev, VL53L1_DISTANCEMODE_LONG);// Set ranging mode;
status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(Dev, 50000); // Set the maximum wait time based on the measurement mode.
status = VL53L1_SetInterMeasurementPeriodMilliSeconds(Dev, 100); // Set the measurement interval.
status = VL53L1_SetUserROI(Dev, &Roi0); //Set measurement area ROI
status = VL53L1_StartMeasurement(Dev); //Start the measurement
if(status) {
printf("VL53L1_StartMeasurement failed \n");
while(1);
}
}
Note: except the step VL53L1_SetUserROI, the other initialization steps can not be skipped.
VL53LXX Ranging Step
^^^^^^^^^^^^^^^^^^^^
The ranging can be done in two kinds of modes: polling mode and interrupt mode.
**Polling Mode Workflow**
.. figure:: ../../_static/vl53lxx_meaturement_sequence.png
:align: center
:alt: vl53lxx_meaturement_sequence
:figclass: align-center
VL53LXX Measurement Sequence
.. note::
- Each time when the measurement and value reading are done, clear the interrupt flag using ``VL53L1_ClearInterruptAndStartMeasurement``, and then start the measurement again.
- Polling mode provides two methods as shown in the figure above: Drivers polloing mode and Host polling mode. We take the Drivers polling mode as an example.
.. code:: text
/* Autonomous ranging loop*/
static void
AutonomousLowPowerRangingTest(void)
{
printf("Autonomous Ranging Test\n");
static VL53L1_RangingMeasurementData_t RangingData;
VL53L1_UserRoi_t Roi1;
int roi = 0;
float left = 0, right = 0;
if (0/*isInterrupt*/) {
} else {
do // polling mode
{
int status = VL53L1_WaitMeasurementDataReady(Dev); // Waiting for the measurement result
if(!status) {
status = VL53L1_GetRangingMeasurementData(Dev, &RangingData); // Get a single measurement data
if(status==0) {
if (roi & 1) {
left = RangingData.RangeMilliMeter;
printf("L %3.1f R %3.1f\n", right/10.0, left/10.0);
} else
right = RangingData.RangeMilliMeter;
}
if (++roi & 1) {
status = VL53L1_SetUserROI(Dev, &Roi1);
} else {
status = VL53L1_SetUserROI(Dev, &Roi0);
}
status = VL53L1_ClearInterruptAndStartMeasurement(Dev); // Release the interrupt
}
}
while (1);
}
// return status;
}
**Interrupt Mode Workflow**
Interrupt is controlled by the pin GPIO1. When the data is ready, pulling down GPIO1 can inform the host to read the data.
.. figure:: ../../_static/vl53lxx_sequence.png
:align: center
:alt: vl53lxx autonomous sequence
:figclass: align-center
VL53LXX Autonomous Sequence
VL53LXX Sensor Calibration
^^^^^^^^^^^^^^^^^^^^^^^^^^
If a mask is installed over the sensor receiver, or if the sensor is mounted behind a transparent cover, the sensor needs to be calibrated due to changes in the transmission rate. You can write a calibration program and call APIs based on the calibration process, or you can measure the calibration value directly using the official PC GUI.
**Use Official APIs to Write Calibration Program**
Calibration process is shown below. The order of calls should be exactly the same.
.. figure:: ../../_static/vl53lxx_calibration_sequence.png
:align: center
:alt: vl53lxx_calibration_sequence
:figclass: align-center
VL53LXX Calibration Sequence
::
/* VL53L1 Module Calibration*/
static VL53L1_CalibrationData_t vl53l1_calibration(VL53L1_Dev_t *dev)
{
int status;
int32_t targetDistanceMilliMeter = 703;
VL53L1_CalibrationData_t calibrationData;
status = VL53L1_WaitDeviceBooted(dev);
status = VL53L1_DataInit(dev); // Device initialization
status = VL53L1_StaticInit(dev); // Load device settings for a given use case.
status = VL53L1_SetPresetMode(dev,VL53L1_PRESETMODE_AUTONOMOUS);
status = VL53L1_PerformRefSpadManagement(dev);
status = VL53L1_PerformOffsetCalibration(dev,targetDistanceMilliMeter);
status = VL53L1_PerformSingleTargetXTalkCalibration(dev,targetDistanceMilliMeter);
status = VL53L1_GetCalibrationData(dev,&calibrationData);
if (status)
{
ESP_LOGE(TAG, "vl53l1_calibration failed \n");
calibrationData.struct_version = 0;
return calibrationData;
}else
{
ESP_LOGI(TAG, "vl53l1_calibration done ! version = %u \n",calibrationData.struct_version);
return calibrationData;
}
}
**Use the Official PC GUI to Calibrate the Sensor**
STM has provided a PC GUI to configure and calibrate the sensor.
- Connect the sensor to ``STM32F401RE nucleo`` development board provided by STM.
- Use softwere to conduct calibration, and get the reference value.
- Enter this value during initialization.
.. figure:: ../../_static/vl53lxx_calibrate_gui.png
:align: center
:alt: STSW-IMG008
:figclass: align-center
PC GUI Calibration
For more information, see `STSW-IMG008: Windows Graphical User Interface (GUI) for VL53L1X Nucleo packs. Works with P-NUCLEO-53L1A1 <https://www.st.com/content/st_com/en/products/embedded-software/proximity-sensors-software/stsw-img008.html>`__\.
VL53L1X Example
^^^^^^^^^^^^^^^^
**Example Description**
- Implementation: VL53L1X detects the height changes (last for 1 second) and the red light turns on. Height returns to normal value (last for 1 second), and the green light turns on.
- Parameters: set the I2C number, port number, LED port number via ``make menuconfig``.
- Example analysis can be found in code notes and user manual.
**Notes**
- This example applies only to VL53L1X, not to its older version hardware VL53L0X.
- According to STM document, the measurement distance is 400 cm in dark environment. In indoor environment, the measurement distance can be 10 to 260 cm.
- Some of the parameters in the initialization function ``vl53l1\_init (VL53L1\_Dev\_t \*)`` need to be determined according to the actual usage environment, and there is room for optimization.
- Make sure the sensor is installed right above the detection position.
- Base height is automatically calibrated when the module is powered on. If the base height changes, the parameters need to be reset again.
**Example Repository**
Click `esp32-vl53l1x-test <https://github.com/qljz1993/esp32-vl53l1x-test/tree/master>`__ to check the example, or download the example using git tool:
.. code:: text
git clone https://github.com/qljz1993/esp32-vl53l1x-test.git
SPI Driver
-----------
PMW3901 Sensor
~~~~~~~~~~~~~~
Overview
^^^^^^^^^^
The PMW3901 is PixArt's latest high-precision, low-power optical navigation module that provides
X-Y motion information with a wide range of 8 cm to infinity. The PWM3901 works with an operating current of less than 9 mA, an operating voltage of VDD (1.8 to 2.1 V), a VDDIO (1.8 to 3.6 V), and uses a 4-wire SPI interface for communication.
**Main Parameters**
================== =======================================
Parameter Value
================== =======================================
Supply voltage (V) VDD: 1.8 ~ 2.1 V; VDDIO: 1.8 ~ 3.6 V
Working range (mm) 80 ~ +∞
Interface 4-line SPI @ 2 MHz
Package 28-pin COB package, size: 6 x 6 x 2.28 mm
================== =======================================
**Package and Pin Layout**
.. figure:: ../../_static/pmw3901_package.png
:align: center
:alt: pmw3901_package
:figclass: align-center
PMW3901 Package
.. figure:: ../../_static/pmw3901_pinmap.png
:align: center
:alt: pmw3901_pinmap
:figclass: align-center
PMW3901 Pinmap
The sensor works at a low operating voltage and to communicates with the ESP32 at 3.3 V requires different voltages from VDD and VDDIO.
Power States and Sequence
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
**Power-Up Sequence**
Although PMW3901MB performs an internal power up self-reset, it is still recommended that the Power_Up_Reset register is written every time power is applied. The appropriate sequence is as follows:
1. Apply power to VDDIO first and followed by VDD, with a delay of no more than 100 ms in between each supply. Ensure all supplies are stable.
2. Wait for at least 40 ms.
3. Drive NCS high, and then low to reset the SPI port.
4. Write 0x5A to Power_Up_Reset register (or alternatively, toggle the NRESET pin).
5. Wait for at least 1 ms.
6. Read from registers 0x02, 0x03, 0x04, 0x05, and 0x06 one time regardless of the motion pin state.
7. Refer to PWM3901MB Datasheet Section 8.2 Performance Optimization Registers to configure the required registers to achieve optimum performance of the chip.
**Power-Down Sequence**
PMW3901MB can be set to Shutdown mode by writing to Shutdown register. The SPI port should not be accessed when Shutdown mode is asserted, except the power-up command (writing 0x5A to register 0x3A). Other ICs on the same SPI bus can be accessed, as long as the chip’s NCS pin is not asserted.
To de-assert Shutdown mode:
1. Drive NCS high, and then low to reset the SPI port.
2. Write 0x5A to Power_Up_Reset register (or alternatively, toggle the NRESET pin).
3. Wait for at least 1 ms.
4. Read from registers 0x02, 0x03, 0x04, 0x05, and 0x06 one time regardless of the motion pin state.
5. Refer to PWM3901MB Datasheet Section 8.2 Performance Optimization Registers to configure the required registers for optimal chip performance.
For more information, see `Connected Home Appliances and IoT <https://www.pixart.com/applications/11/Connected_Home_Appliances_%EF%BC%86_IoT>`__.
Code Interpretation
^^^^^^^^^^^^^^^^^^^^
**Key Structs**
::
typedef struct opFlow_s
{
float pixSum[2]; /*accumulated pixels*/
float pixComp[2]; /*pixel compensation*/
float pixValid[2]; /*valid pixels*/
float pixValidLast[2]; /*last valid pixel*/
float deltaPos[2]; /*displacement between 2 frames, unit: cm*/
float deltaVel[2]; /*velocity, unit: cm/s*/
float posSum[2]; /*accumulated displacement, unit: cm*/
float velLpf[2]; /*low-pass velocity, unit cm/s*/
bool isOpFlowOk; /*optical flow*/
bool isDataValid; /* valid data */
} opFlow_t;
- Accumulated pixels: accumulated pixels after the drone takes off.
- Pixel compensation: compensation for pixel error caused by drone tilt.
- Valid pixels: the actual pixels that have been compensated.
- Displacement between 2 frames: the actual displacement converted from pixels, unit: cm.
- Velocity: instantaneous velocity, obtained by differentiating on displacement changes, unit: cm/s.
- Accumulated displacement: actual displacement, unit: cm.
- Low-pass velocity: low-pass operation on velocity increases data smoothness.
- Optical flow status: check if this optical flow sensor is working properly.
- Valid data: data is valid at a certain height.
::
typedef struct motionBurst_s {
union {
uint8_t motion;
struct {
uint8_t frameFrom0 : 1;
uint8_t runMode : 2;
uint8_t reserved1 : 1;
uint8_t rawFrom0 : 1;
uint8_t reserved2 : 2;
uint8_t motionOccured : 1;
};
};
uint8_t observation;
int16_t deltaX;
int16_t deltaY;
uint8_t squal;
uint8_t rawDataSum;
uint8_t maxRawData;
uint8_t minRawData;
uint16_t shutter;
} __attribute__((packed)) motionBurst_t;
- motion: motion information, can be obtained according to the bits frame detection (frameFrom0), operating mode (runMode) and motion detection (motionOccured).
- observation: to check if the IC has EFT/B or ESD issues. When the sensor works properly, the value should be 0xBF.
- deltaX and deltaY: the sensor has detected the motion of the image at X and Y directions.
- squal: the quality of motion information, i.e, the credibility of motion information;
- rawDataSum: the sum of raw data, can be used to average the data of a frame.
- maxRawData and minRawData: the maximum and minimum of the raw data;
- shutter: a real-time auto-adjusted value, to ensure that the average motion data remains within the normal operating range. Shutter can be used together with ``squal`` to check whether the motion information is available.
Programming Notes
^^^^^^^^^^^^^^^^^^^^
- If the sensor data is 0 and lasts for 1 second, it indicates that an error occurs. If so, optical flow tasks should be suspended.
- The sensor lens must be mounted facing down. Due to relative motion, the displacement data collected by the sensor is opposite to the actual motion direction of the drone.
- Enable position-hold mode only when the height-hold mode test is stable. Accurate height information is used to determine the relation between image pixels and actual distance.
- Manual test on tilt compensation can remain the sensor output nearly unchanged even when the drone tilts on certain direction.
- With tilt compensation and motion accumulated pixels, the actual accumulated pixels can be obtained. After several calculations, you can get:
- Pixel changes between 2 frames = actual accumulated pixels - last actual pixels;
- Displacement changes between 2 frames = Pixel changes between 2 frames x coefficient. Note when the height is less than 5 cm, the optical flow will stop working, so the coefficient should be set to 0;
- Integrate on the above displacement changes, to obtain the displacement from the four axes to the take-off point. Differentiate on the above displacement changes, to obtain the instantaneous velocity. Conduct low-pass operation on velocity, to increase data smoothness. Limit amplitude on velocity, to enhance data security.
- The position and velocity information of the four axes are obtained through the optical flow, and then:
- The above location and speed information together with the accelerometer (state_estimator.c) can be used to estimate the position and speed;
- Estimated position and speed are involved in PID operations and can be used for horizontal position control. Refer to position_pid.c to see how the position ring and speed ring PID are handled.
Finally, horizontal position-hold control can be achieved through the above process.
docs/en/rst/getespidf.rst
+6 -3
View File
@@ -1,12 +1,13 @@
Set up Development Environment
================================
:link_to_translation:`zh_CN:[中文]`
Set up ESP-IDF Environment
---------------------------------
Please refer to `ESP-IDF Programming Guide <https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html>`__\ and set up ESP-IDF environmnet step by step.
Note：
.. note::
- Please follow the link above and complete all the steps.
- Build one or more sample applications by following the steps in the link above.
@@ -50,9 +51,11 @@ Get Project Source Code
**The code file structure is as follows:**
.. figure:: ../../_static/espdrone_file_structure.png
:align: center
:alt: espdrone_file_structure
:figclass: align-center
espdrone_file_structure
ESP-Drone File Structure
::
@@ -92,7 +95,7 @@ Get Project Source Code
├── README.md | project description
└── sdkconfig.defaults | default parameter
**For more information, please refer to**\ ：\ `espdrone_file_structure <./_static/espdrone_file_structure.pdf>`__\
**For more information, please refer to**: `espdrone_file_structure <./_static/espdrone_file_structure.pdf>`__.
Source Code Style
--------------------
docs/en/rst/gettingstarted.rst
+15 -15
View File
@@ -21,12 +21,12 @@ license.
Main Features
----------------
ESP-Drone supports the following features:
ESP-Drone has the following features:
- Stabilize mode: keep the drone stable to achieve smooth flight.
- Height-hold mode: control thrust output to keep the drone flying at a fixed height.
- Position-hold mode: keep the drone flying at its current position.
- PC debugging: uses cfclient for static/dynamic debugging.
- Position-hold mode: keep the drone flying at a fixed position.
- PC debugging: use cfclient for static/dynamic debugging.
- Controlled by APP: easily controlled over Wi-Fi by your mobile APP.
- Controlled by gamepad: easily controlled via the gamepad by cfclient.
@@ -36,7 +36,7 @@ Main Components
ESP-Drone 2.0 consists of a main board and several extension boards:
- **Main board**: integrates an ESP32-S2 module, necessary sensors for basic flight, and provides hardware extension interfaces.
- **Extension boards**: integrates extension sensors via hardware extension interfaces of the main board, to implement advanced flight.
- **Extension boards**: integrate extension sensors via hardware extension interfaces of the main board, to implement advanced flight.
.. list-table::
:widths: 4 20 15 15 10 10
@@ -52,7 +52,7 @@ ESP-Drone 2.0 consists of a main board and several extension boards:
- Main board - **ESP32-S2**
- ESP32-S2-WROVER + MPU6050
- Basic flight
- I2C, SPI, GPIO extension interfaces
- I2C, SPI, GPIO, extension interfaces
-
* - 2
- Extension board - **Position-hold module**
@@ -129,7 +129,7 @@ Download and Install ESP-Drone APP
----------------------------------------
ESP-Drone APP is available for Android and iOS.
For Android, please scan the QR below to download ESP-Drone APP:
For Android, please scan the QR below to download ESP-Drone APP.
.. figure:: ../../_static/android_app_download.png
:align: center
@@ -311,7 +311,7 @@ Basic Flight Control
2. Assisted mode
- Altitude-hold mode: maintain flight altitude. To implement this mode, a barometric pressure sensor is needed.
- Position-hold mode: maintain current flight position. To implement this mode, a optical flow sensor and a Time of Flight (TOF) sensor are needed.
- Position-hold mode: maintain current flight position. To implement this mode, an optical flow sensor and a Time of Flight (TOF) sensor are needed.
- Height-hold mode: keep flight height. Note: to apply this mode, the drone should fly at 40 cm or higher over the ground, and a TOF is needed.
- Hover mode: stay and hover at 40 cm or higher over the take-off point. To implement this mode, a optical flow sensor and a TOF are needed.
@@ -329,7 +329,7 @@ Advanced Flight Control
2. Max yaw rate: set the allowed yaw: ``yaw``.
3. Max thrust: set the maximum thrust.
4. Min thrust: set the minimum thrust.
5. Slew limit: prevent sudden drop of thrust. When the thrust drops below this limit, the rates below `` Slew rate`` will not be allowed.
5. Slew limit: prevent sudden drop of thrust. When the thrust drops below this limit, the rates below ``Slew rate`` will not be allowed.
6. Slew rate: this is the maximum rate when the thrust is below ``slew limit``.
Configure Input Device
@@ -344,8 +344,8 @@ Follow the prompts, route the controllers to each channel.
Cfclient Input Device Configuration
Monitor Flight Data
~~~~~~~~~~~~~~~~~~~~~~~~~~
Flight Data
~~~~~~~~~~~~~~~~
On the tab “Flight Control” of cfclient, you can check the drone status. The detailed information is shown at the bottom right, including:
@@ -355,9 +355,9 @@ On the tab “Flight Control” of cfclient, you can check the drone status. The
4. M1/M2/M3/M4: actual output of motors
Tune Online Parameters
----------------------------------------
--------------------------
**Adjust PID parameters online**
**Tune PID parameters online**
.. figure:: ../../_static/cfclient_pid_tune.png
:align: center
@@ -371,11 +371,11 @@ Tune Online Parameters
1. The modified parameters take effect in real time, which avoids frequent flash of firmware.
2. You can define in your code which parameters can be modified by PC in real time.
3. Note that modifying parameters online is only for debugging purpose. The modified parameters will not be saved.
3. Note that modifying parameters online is only for debugging purpose. The modified parameters will not be saved at power down.
Flight Data Monitoring
-------------------------
Monitor Flight Data
-----------------------
Configure the parameters to monitor at Tab Log configuration and Tab Log Blocks:
docs/en/rst/hardware.rst
+28 -20
View File
@@ -1,5 +1,6 @@
Hardware Reference
=======================
:link_to_translation:`zh_CN:[中文]`
Supported Hardware
----------------------
@@ -21,13 +22,13 @@ Configure Target Hardware
- Code in ``esp_drone`` repository supports a wide variety of hardware, which can be changed through ``menuconfig``.
.. figure:: ../../_static/board_choose.png
:align: center
:alt: ESP-Drone config
:figclass: align-center
ESP-Drone config
- By default, when set ``set-target`` as ``esp32s2``, target hardware will be changed to ``ESP32_S2_Drone_V1_2`` automatically.
- By default, when set \ ``set-target`` as ``esp32s2``\, target hardware will be changed to ``ESP32_S2_Drone_V1_2``\ automatically.
- By default, when set \ ``set-target`` as ``esp32``, target hardware will be changed to ``ESPlane_FC_V1``\ automatically.
- By default, when set ``set-target`` as ``esp32``, target hardware will be changed to ``ESPlane_FC_V1`` automatically.
**Notes**
@@ -47,13 +48,14 @@ ESP32-S2-Drone V1.2
-----------------------
.. figure:: ../../_static/espdrone_s2_v1_2_up2.jpg
:align: center
:alt: ESP-Drone
:figclass: align-center
ESP-Drone
ESP32-S2-Drone V1.2 Overview
Main Board Schematic：\ `SCH_Mainboard_ESP32_S2_Drone_V1_2 <./_static/ESP32_S2_Drone_V1_2/SCH_Mainboard_ESP32_S2_Drone_V1_2.pdf>`__
Main Board PCB：\ `PCB_Mainboard_ESP32_S2_Drone_V1_2 <./_static/ESP32_S2_Drone_V1_2/PCB_Mainboard_ESP32_S2_Drone_V1_2.pdf>`__
- Main Board Schematic：`SCH_Mainboard_ESP32_S2_Drone_V1_2 <./_static/ESP32_S2_Drone_V1_2/SCH_Mainboard_ESP32_S2_Drone_V1_2.pdf>`__
- Main Board PCB：`PCB_Mainboard_ESP32_S2_Drone_V1_2 <./_static/ESP32_S2_Drone_V1_2/PCB_Mainboard_ESP32_S2_Drone_V1_2.pdf>`__
Basic Component
~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -62,9 +64,11 @@ Basic Component List
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
.. figure:: ../../_static/espdrone_s2_v1_2_hardware_package.png
:align: center
:alt: ESP-Drone
:figclass: align-center
ESP-Drone
Component List for ESP32-S2-Drone V1.2
================================== ======== =====================================
Basic Component List Number Notes
@@ -79,7 +83,9 @@ Main board 1 ESP32-S2-WROVER + MPU6050
8-pin 25 mm male pins 2
================================== ======== =====================================
Notice: Set ``motor type`` as ``brushed 720 motor`` through ``menuconfig->ESPDrone Config->motors config`` if you use 720 motor.
.. note::
Set ``motor type`` as ``brushed 720 motor`` through ``menuconfig->ESPDrone Config->motors config`` if you use 720 motor.
Main Controller
^^^^^^^^^^^^^^^^^^^^^^^^
@@ -199,9 +205,9 @@ Extension Components
- Mount at the top or at the bottom
Extension board schematics：to be released
.. Extension board schematics：to be released
Extension board PCB：to be released
.. Extension board PCB：to be released
Definition of Extension Board IO
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -223,25 +229,27 @@ ESPlane-V2-S2
-------------
.. figure:: ../../_static/esplane_2_0.jpg
:align: center
:alt: esplane_fc_v1
:figclass: align-center
esplane_fc_v1
ESPlane-V2-S2 Overview
Main Board Schematic：\ `SCH_ESPlane_V2_S2 <./_static/ESPlane_V2_S2/SCH_ESPlane_V2_S2.pdf>`__
Main Board PCB：\ `PCB_ESPlane_V2_S2 <./_static/ESPlane_V2_S2/PCB_ESPlane_V2_S2.pdf>`__
- Main Board Schematic：`SCH_ESPlane_V2_S2 <./_static/ESPlane_V2_S2/SCH_ESPlane_V2_S2.pdf>`__
- Main Board PCB：`PCB_ESPlane_V2_S2 <./_static/ESPlane_V2_S2/PCB_ESPlane_V2_S2.pdf>`__
ESPlane-FC-V1
------------------
.. figure:: ../../_static/esplane_1_0.jpg
:align: center
:alt: esplane_fc_v1
:figclass: align-center
esplane_fc_v1
Main Board Schematic：\ `Schematic_ESPlane_FC_V1 <./_static/ESPlane_FC_V1/Schematic_ESPlane_FC_V1.pdf>`__
Main Board PCB：\ `PCB_ESPlane_FC_V1 <./_static/ESPlane_FC_V1/PCB_ESPlane_FC_V1.pdf>`__
- Main Board Schematic：`Schematic_ESPlane_FC_V1 <./_static/ESPlane_FC_V1/Schematic_ESPlane_FC_V1.pdf>`__
- Main Board PCB：`PCB_ESPlane_FC_V1 <./_static/ESPlane_FC_V1/PCB_ESPlane_FC_V1.pdf>`__
.. _Basic_Component-1:
@@ -327,7 +335,7 @@ GPIO35 ADC_7_BAT VBAT/2
.. _Components_of_extension_board-1:
Componets of Extension Board
Components of Extension Board
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ESPlane + PMW3901 Pins Allocation
docs/en/rst/index.rst
+1
View File
@@ -1,6 +1,7 @@
ESP-Drone
===========
:link_to_translation:`zh_CN:[中文]`
**ESP-Drone** is an open source **drone solution** based on Espressif **ESP32/ESP32-S2** Wi-Fi chip, which can be controlled over a **Wi-Fi** network using a mobile APP or gamepad. ESP-Drone supports multiple flight modes, including `Stabilize mode`, `Height-hold mode`, and `Position-hold mode`. With **simple hardware**, **clear and extensible code architecture**, ESP-Drone can be used in **STEAM education** and other fields. The main code is ported from **Crazyflie** open source project with **GPL3.0** protocol.
docs/en/rst/notice.rst
+19 -2
View File
@@ -1,3 +1,20 @@
Third-Party Codes
======================
:link_to_translation:`zh_CN:[中文]`
The third-party codes adopted in ESP-Drone project and the licenses are as follows:
============== ========= ======================================================================================= ========================================
Components Licenses Source Codes Commit ID
============== ========= ======================================================================================= ========================================
core/crazyflie GPL-3.0 `Crazyflie <https://github.com/bitcraze/crazyflie-firmware>`__ a2a26abd53a5f328374877bfbcb7b25ed38d8111
lib/dsp_lib `esp32-lin <https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib>`__ 6fa39f4cd5f7782b3a2a052767f0fb06be2378ff
============== ========= ======================================================================================= ========================================
Acknowledgement
================
第三方代码和致谢
================
1. Thanks Bitcraze for providing the amazing drone codes: `Crazyflie <https://www.bitcraze.io/%20>`__
2. Thanks Espressif for providing ESP32 and `ESP-IDF <https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html>`__\
3. Thanks WhyEngineer for providing DSP lib `esp-dsp <https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib>`__\ 。
docs/en/rst/system.rst
+617 -2
View File
@@ -1,3 +1,618 @@
======================
Flight Control System
======================
=======================
:link_to_translation:`zh_CN:[中文]`
Startup Process
----------------
.. figure:: ../../_static/start_from_app_main.png
:align: center
:alt: start_from_app_main
:figclass: align-center
For source file, please check `start_from_app_main <./_static/start_from_app_main.pdf>`__.
Task Management
----------------
Tasks
~~~~~~
The following TASKs are started when the system is operating properly.
.. figure:: ../../_static/task_dump.png
:align: center
:alt: task_dump
:figclass: align-center
Task Dump
* Load: CPU occupancy
* Stack Left: remaining stack space
* Name: task name
* PRI: task priority
TASKs are described as follows:
- PWRMGNT: monitor system voltage
- CMDHL: application layer - process advanced commands based on CRTP protocol
- CRTP-RX: protocol layer - decode CRTP flight protocol
- CRTP-TX: protocol layer - decode CRTP flight protocol
- UDP-RX: transport layer - receive UDP packet
- UDP-TX: Transport Layer - send UDP packet
- WIFILINK: work with CRTP protocol layer and UDP transport layer
- SENSORS: read and pre-process sensor data
- KALMAN: estimate the drone’s status according to sensor data, including the drone's angle, angular velocity, and spatial position. This TASK consumes a large amount of CPU resources on the ESP chip and users should be careful about its priority allocation.
- PARAM: modify variables remotely according to CRTP protocol
- LOG: monitor variables on real-time according to CRTP protocol
- MEM: modify memory remotely according to CRTP protocol
- STABILIZER: self stabilize its thread, and control the process of flight control program
- SYSTEM: control system initialization and self-test process
Configure Task Stack Size
~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Users can modify the stack size in ``components/config/include/config.h``, or modify ``BASE_STACK_SIZE`` in ``menucfg``. When the target is ``ESP32``, you can adjust the ``BASE_STACK_SIZE`` to 2048, to avoid memory overlapping. When the target is ``ESP32-S2``, modify the value to ``1024``\.
::
//Task stack size
#define SYSTEM_TASK_STACKSIZE (4* configBASE_STACK_SIZE)
#define ADC_TASK_STACKSIZE configBASE_STACK_SIZE
#define PM_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define CRTP_TX_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define CRTP_RX_TASK_STACKSIZE (2* configBASE_STACK_SIZE)
#define CRTP_RXTX_TASK_STACKSIZE configBASE_STACK_SIZE
#define LOG_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define MEM_TASK_STACKSIZE (1 * configBASE_STACK_SIZE)
#define PARAM_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define SENSORS_TASK_STACKSIZE (2 * configBASE_STACK_SIZE)
#define STABILIZER_TASK_STACKSIZE (2 * configBASE_STACK_SIZE)
#define NRF24LINK_TASK_STACKSIZE configBASE_STACK_SIZE
#define ESKYLINK_TASK_STACKSIZE configBASE_STACK_SIZE
#define SYSLINK_TASK_STACKSIZE configBASE_STACK_SIZE
#define USBLINK_TASK_STACKSIZE configBASE_STACK_SIZE
#define WIFILINK_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define UDP_TX_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define UDP_RX_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define UDP_RX2_TASK_STACKSIZE (1*configBASE_STACK_SIZE)
#define PROXIMITY_TASK_STACKSIZE configBASE_STACK_SIZE
#define EXTRX_TASK_STACKSIZE configBASE_STACK_SIZE
#define UART_RX_TASK_STACKSIZE configBASE_STACK_SIZE
#define ZRANGER_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define ZRANGER2_TASK_STACKSIZE (2* configBASE_STACK_SIZE)
#define FLOW_TASK_STACKSIZE (2* configBASE_STACK_SIZE)
#define USDLOG_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define USDWRITE_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define PCA9685_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define CMD_HIGH_LEVEL_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define MULTIRANGER_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define ACTIVEMARKER_TASK_STACKSIZE configBASE_STACK_SIZE
#define AI_DECK_TASK_STACKSIZE configBASE_STACK_SIZE
#define UART2_TASK_STACKSIZE configBASE_STACK_SIZE
Configure Task Priority
~~~~~~~~~~~~~~~~~~~~~~~~~~~~
The priority of system tasks can be configured in ``components/config/include/config.h``. ``ESP32``, for its dual-core advantage, has more computing resources than ``ESP32-S2``, therefore, its time-consuming ``KALMAN_TASK`` can be set to a higher priority. But if the target is ``ESP32-S2``, please lower the priority of the ``KALMAN_TASK``, otherwise task watchdog will be triggered due to the failure of releasing enough CPU resources.
::
//Task priority: the higher the number, the higher the priority.
#define STABILIZER_TASK_PRI 5
#define SENSORS_TASK_PRI 4
#define ADC_TASK_PRI 3
#define FLOW_TASK_PRI 3
#define MULTIRANGER_TASK_PRI 3
#define SYSTEM_TASK_PRI 2
#define CRTP_TX_TASK_PRI 2
#define CRTP_RX_TASK_PRI 2
#define EXTRX_TASK_PRI 2
#define ZRANGER_TASK_PRI 2
#define ZRANGER2_TASK_PRI 2
#define PROXIMITY_TASK_PRI 0
#define PM_TASK_PRI 0
#define USDLOG_TASK_PRI 1
#define USDWRITE_TASK_PRI 0
#define PCA9685_TASK_PRI 2
#define CMD_HIGH_LEVEL_TASK_PRI 2
#define BQ_OSD_TASK_PRI 1
#define GTGPS_DECK_TASK_PRI 1
#define LIGHTHOUSE_TASK_PRI 3
#define LPS_DECK_TASK_PRI 5
#define OA_DECK_TASK_PRI 3
#define UART1_TEST_TASK_PRI 1
#define UART2_TEST_TASK_PRI 1
//if task watchdog triggered, KALMAN_TASK_PRI should set lower or set lower flow frequency
#ifdef TARGET_MCU_ESP32
#define KALMAN_TASK_PRI 2
#define LOG_TASK_PRI 1
#define MEM_TASK_PRI 1
#define PARAM_TASK_PRI 1
#else
#define KALMAN_TASK_PRI 1
#define LOG_TASK_PRI 2
#define MEM_TASK_PRI 2
#define PARAM_TASK_PRI 2
#endif
#define SYSLINK_TASK_PRI 3
#define USBLINK_TASK_PRI 3
#define ACTIVE_MARKER_TASK_PRI 3
#define AI_DECK_TASK_PRI 3
#define UART2_TASK_PRI 3
#define WIFILINK_TASK_PRI 3
#define UDP_TX_TASK_PRI 3
#define UDP_RX_TASK_PRI 3
#define UDP_RX2_TASK_PRI 3
Key Tasks
----------
Except the system default enabled tasks, such as Wi-Fi TASK, the task with the highest priority is ``STABILIZER_TASK``, highlighting the importance of this task. ``STABILIZER_TASK`` controls the entire process from sensor data reading, attitude calculation, target receiving, to final motor power output, and drives the algorithms at each stage.
.. figure:: ../../_static/General-framework-of-the-stabilization-structure-of-the-crazyflie-with-setpoint-handling.png
:align: center
:alt: stabilizerTask process
:figclass: align-center
stabilizerTask Process
.. figure:: ../../_static/stabilizerTask.png
:align: center
:alt: stabilizerTask
:figclass: align-center
stabilizerTask
Sensor Driver
--------------
The sensor driver code can be found in ``components\drivers``. ``drivers`` applies a file structure similar to that used in `esp-iot-solution <https://github.com/espressif/esp-iot-solution/>`__. In such structure, drivers are classified by the bus they belong to, including ``i2c_devices``, ``spi_devices``, and ``general``. For more information, please refer to `Drivers <./drivers>`__.
.. figure:: ../../_static/drivers_flie_struture.png
:align: center
:alt: drivers_flie_struture
:figclass: align-center
Drivers File Structure
Sensor Hardware Abstraction
----------------------------
``components\core\crazyflie\hal\src\sensors.c`` provides hardware abstraction for the sensors. Users are free to combine sensors to interact with upper-level application by implementing the sensor interfaces defined by the hardware abstraction layer.
::
typedef struct {
SensorImplementation_t implements;
void (*init)(void);
bool (*test)(void);
bool (*areCalibrated)(void);
bool (*manufacturingTest)(void);
void (*acquire)(sensorData_t *sensors, const uint32_t tick);
void (*waitDataReady)(void);
bool (*readGyro)(Axis3f *gyro);
bool (*readAcc)(Axis3f *acc);
bool (*readMag)(Axis3f *mag);
bool (*readBaro)(baro_t *baro);
void (*setAccMode)(accModes accMode);
void (*dataAvailableCallback)(void);
} sensorsImplementation_t;
The sensor abstraction interfaces implemented by ESP-Drone are listed in ``components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c``, which can interact with the upper-level application by the following assignment process.
::
#ifdef SENSOR_INCLUDED_MPU6050_HMC5883L_MS5611
{
.implements = SensorImplementation_mpu6050_HMC5883L_MS5611,
.init = sensorsMpu6050Hmc5883lMs5611Init,
.test = sensorsMpu6050Hmc5883lMs5611Test,
.areCalibrated = sensorsMpu6050Hmc5883lMs5611AreCalibrated,
.manufacturingTest = sensorsMpu6050Hmc5883lMs5611ManufacturingTest,
.acquire = sensorsMpu6050Hmc5883lMs5611Acquire,
.waitDataReady = sensorsMpu6050Hmc5883lMs5611WaitDataReady,
.readGyro = sensorsMpu6050Hmc5883lMs5611ReadGyro,
.readAcc = sensorsMpu6050Hmc5883lMs5611ReadAcc,
.readMag = sensorsMpu6050Hmc5883lMs5611ReadMag,
.readBaro = sensorsMpu6050Hmc5883lMs5611ReadBaro,
.setAccMode = sensorsMpu6050Hmc5883lMs5611SetAccMode,
.dataAvailableCallback = nullFunction,
}
#endif
Sensor Calibration
--------------------
Gyroscope Calibration
~~~~~~~~~~~~~~~~~~~~~~~~
Due to large temperature drift, the gyroscope needs to be calibrated before each use, to calculate its reference values in current environment. ESP-Drone continues the gyroscope calibration scheme provided by Crazyflie 2.0. At the first power-up, the variance and average at the three axes of the gyroscope are calculated.
The detailed gyroscope calibration is as follows:
1. Store the latest 1024 sets of gyroscope measurements into a ring buffer with a maximum length of 1024.
2. Calculate the variance of the gyroscope output values, to check whether the drone is placed level and gyroscope is working properly.
3. If Step 2 is OK, calculate the average of the 1024 sets of the gyroscope output values as its calibration value.
Below is the source code for gyroscope base calculation:
::
/**
* Adds a new value to the variance buffer and if it is full
* replaces the oldest one. Thus a circular buffer.
*/
static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z)
{
bias->bufHead->x = x;
bias->bufHead->y = y;
bias->bufHead->z = z;
bias->bufHead++;
if (bias->bufHead >= &bias->buffer[SENSORS_NBR_OF_BIAS_SAMPLES])
{
bias->bufHead = bias->buffer;
bias->isBufferFilled = true;
}
}
/**
* Checks if the variances is below the predefined thresholds.
* The bias value should have been added before calling this.
* @param bias The bias object
*/
static bool sensorsFindBiasValue(BiasObj* bias)
{
static int32_t varianceSampleTime;
bool foundBias = false;
if (bias->isBufferFilled)
{
sensorsCalculateVarianceAndMean(bias, &bias->variance, &bias->mean);
if (bias->variance.x < GYRO_VARIANCE_THRESHOLD_X &&
bias->variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
bias->variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
(varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
{
varianceSampleTime = xTaskGetTickCount();
bias->bias.x = bias->mean.x;
bias->bias.y = bias->mean.y;
bias->bias.z = bias->mean.z;
foundBias = true;
bias->isBiasValueFound = true;
}
}
return foundBias;
}
**Trim Gyroscope Output Values**
::
sensorData.gyro.x = (gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
applyAxis3fLpf((lpf2pData *)(&gyroLpf), &sensorData.gyro); // LPF Filter, to avoid high-frequency interference
Accelerometer Calibration
~~~~~~~~~~~~~~~~~~~~~~~~~
Gravitational Acceleration (g) Calibration
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
The values of g are generally different at various latitudes and altitudes of the Earth, so an accelerometer is required to measure the actual g. The accelerometer calibration scheme provided by Crazyflie 2.0 can be your reference, and its g-value calibration process is as follows:
1. Once the gyroscope calibration is complete, the accelerometer calibration is performed immediately.
2. Store 200 sets of accelerometer measurements to the Buffer.
3. Calculate the value of g at rest by synthesizing the weight of g on three axes.
For more information, see `g Values at Various Latitudes and Altitudes <https://baike.baidu.com/item/%E9%87%8D%E5%8A%9B%E5%8A%A0%E9%80%9F%E5%BA%A6/23553>`__\.
**Calculate g values at rest**
::
/**
* Calculates accelerometer scale out of SENSORS_ACC_SCALE_SAMPLES samples. Should be called when
* platform is stable.
*/
static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
{
static bool accBiasFound = false;
static uint32_t accScaleSumCount = 0;
if (!accBiasFound)
{
accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
accScaleSumCount++;
if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
{
accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
accBiasFound = true;
}
}
return accBiasFound;
}
**Trim accelerometer measurements by the actual g values**
::
accScaled.x = (accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
accScaled.y = (accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
accScaled.z = (accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;
Calibrate the Drone at Horizontal Level
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Ideally, the accelerometer is installed horizontally on the drone, allowing the 0 position to be used as the drone's horizontal surface. However, due to the inevitable inclination of the accelerometer when installed, the flight control system can not estimate the horizontal position accurately, resulting in the drone flying in a certain direction. Therefore, a certain calibration strategy needs to be set to balance this error.
1. Place the drone on a horizontal surface, and calculate ``cosRoll``, ``sinRoll``, ``cosPitch``, and ``sinPitch``. Ideally, ``cosRoll`` and ``cosPitch`` are 1. ``sinPitch`` and ``sinRoll`` are 0. If the accelerometer is not installed horizontally, ``sinPitch`` and ``sinRoll`` are not 0. ``cosRoll`` and ``cosPitch`` are not 1.
2. Store the ``cosRoll``, ``sinRoll``, ``cosPitch``, and ``sinPitch`` obtained in Step 1, or the corresponding ``Roll`` and ``Pitch`` to the drone for calibration.
Use the calibration value to trim accelerometer measurements:
::
/**
* Compensate for a miss-aligned accelerometer. It uses the trim
* data gathered from the UI and written in the config-block to
* rotate the accelerometer to be aligned with gravity.
*/
static void sensorsAccAlignToGravity(Axis3f *in, Axis3f *out)
{
//TODO: need cosPitch calculate firstly
Axis3f rx;
Axis3f ry;
// Rotate around x-axis
rx.x = in->x;
rx.y = in->y * cosRoll - in->z * sinRoll;
rx.z = in->y * sinRoll + in->z * cosRoll;
// Rotate around y-axis
ry.x = rx.x * cosPitch - rx.z * sinPitch;
ry.y = rx.y;
ry.z = -rx.x * sinPitch + rx.z * cosPitch;
out->x = ry.x;
out->y = ry.y;
out->z = ry.z;
}
The above process can be deduced via the resolution of a force and Pythagorean Theorem.
Attitude Calculation
----------------------
Supported Attitude Calculation Algorithms
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- Complementary filtering
- Kalman filtering
Attitude calculation used in ESP-Drone is from ``Crazyflie``. ESP-Drone firmware has been tested for complementary filtering and Kalman filtering to efficiently calculate flight attitudes, including the angle, angular velocity, and spatial position, providing a reliable state input for the control system. Note that in position-hold mode, you must switch to Kalman filtering algorithm to ensure proper operation.
Crazyflie Status Estimation can be found in `State estimation: To be or not to be! <https://www.bitcraze.io/2020/01/state-estimation-to-be-or-not-to-be/>`__
Complementary Filtering
~~~~~~~~~~~~~~~~~~~~~~~~
.. figure:: ../../_static/Schematic-overview-of-inputs-and-outputs-of-the-Complementary-filter.png
:align: center
:alt: Extended-Kalman-Filter
:figclass: align-center
Complementary Filtering
Kalman Filtering
~~~~~~~~~~~~~~~~~~~~
.. figure:: ../../_static/Schematic-overview-of-inputs-and-outputs-of-the-Extended-Kalman-Filter.png
:align: center
:alt: Extended-Kalman-Filter
:figclass: align-center
Extended Kalman Filtering
Fight Control Algorithms
--------------------------
Supported Controller
~~~~~~~~~~~~~~~~~~~~~~~~~~~~
The control system code used in ESP-Drone is from ``Crazyflie``, and continues all its algorithms. Please note that ESP-Drone has only tested and tuned the parameters for PID controller. When using other controllers, tune your parameters while ensuring safety.
.. figure:: ../../_static/possible_controller_pathways.png
:align: center
:alt: possible_controller_pathways
:figclass: align-center
Possible Controller Pathways
For more information, please refer to `Out of Control <https://www.bitcraze.io/2020/02/out-of-control/>`__.
In the code, you can modify the input parameters of ``controllerInit(ControllerType controller)`` to switch the controller.
Customized controllers can also be added by implementing the following controller interfaces.
::
static ControllerFcns controllerFunctions[] = {
{.init = 0, .test = 0, .update = 0, .name = "None"}, // Any
{.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"},
{.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"},
{.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"},
};
PID Controller
~~~~~~~~~~~~~~~
**How the PID controller works**
The PID controller (proportional-integral-derivative controller), consists of a proportional unit, an integral unit, and a derivative unit, corresponding to the current error, past cumulative error and future error, respectively, and then controls the system based on the error and the error rate of change. The PID
controller is considered to be the most suitable controller because of its negative feedback correction. By adjusting the PID
controller’s three parameters, you can adjust the speed of the system's response to the error, the degree of the controller’s overswing and shake, so that the system can reach the optimal state.
This drone system has three control dimensions: ``pitch``, ``roll``, and ``yaw``, so it is necessary to design a PID controller with a closed loop as shown in the figure below.
.. figure:: https://img-blog.csdnimg.cn/20190929142813169.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70
:alt: Crazyflie Control System
:figclass: align-center
:align: center
Crazyflie Control System
For each control dimension, a string-level PID controller is provided, consisting of a Rate controller and an Attitude controller. Rate controller is used to control the speed of angle correction based on the input of angular velocity. Attitude controller is used to control the drone to fly at a target angle based on the input of fitting angles. The two controllers can work together at various frequencies. Of course, you can also choose to use only
one single-level PID controller, where pitch and roll control dimensions are controlled by Attitude by default, and yaw
controlled by Rate.
::
You can modify the parameters in crtp_commander_rpyt.c:
static RPYType stabilizationModeRoll = ANGLE; // Current stabilization type of roll (rate or angle)
static RPYType stabilizationModePitch = ANGLE; // Current stabilization type of pitch (rate or angle)
static RPYType stabilizationModeYaw = RATE; // Current stabilization type of yaw (rate or angle)
**Implementation Code**
::
void controllerPid(control_t *control, setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick)
{
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { // This macro controls PID calculation frequency based on the interrupts triggered by MPU6050
// Rate-controled YAW is moving YAW angle setpoint
if (setpoint->mode.yaw == modeVelocity) { //rate mode, correct yaw
attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT;
while (attitudeDesired.yaw > 180.0f)
attitudeDesired.yaw -= 360.0f;
while (attitudeDesired.yaw < -180.0f)
attitudeDesired.yaw += 360.0f;
} else { //attitude mode
attitudeDesired.yaw = setpoint->attitude.yaw;
}
}
if (RATE_DO_EXECUTE(POSITION_RATE, tick)) { //Position control
positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
}
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
// Switch between manual and automatic position control
if (setpoint->mode.z == modeDisable) {
actuatorThrust = setpoint->thrust;
}
if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
attitudeDesired.roll = setpoint->attitude.roll;
attitudeDesired.pitch = setpoint->attitude.pitch;
}
attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
&rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
// For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
// value. Also reset the PID to avoid error buildup, which can lead to unstable
// behavior if level mode is engaged later
if (setpoint->mode.roll == modeVelocity) {
rateDesired.roll = setpoint->attitudeRate.roll;
attitudeControllerResetRollAttitudePID();
}
if (setpoint->mode.pitch == modeVelocity) {
rateDesired.pitch = setpoint->attitudeRate.pitch;
attitudeControllerResetPitchAttitudePID();
}
// TODO: Investigate possibility to subtract gyro drift.
attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
attitudeControllerGetActuatorOutput(&control->roll,
&control->pitch,
&control->yaw);
control->yaw = -control->yaw;
}
if (tiltCompensationEnabled)
{
control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
}
else
{
control->thrust = actuatorThrust;
}
if (control->thrust == 0)
{
control->thrust = 0;
control->roll = 0;
control->pitch = 0;
control->yaw = 0;
attitudeControllerResetAllPID();
positionControllerResetAllPID();
// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = state->attitude.yaw;
}
}
Mellinger Controller
~~~~~~~~~~~~~~~~~~~~~~
Mellinger controller is an **all-in-one** controller that directly calculates the required thrust allocated to all motors, based on the target position and the speed vector on the target position.
For your reference: `Minimum snap trajectory generation and control for
quadrotors <https://ieeexplore.ieee.org/abstract/document/5980409>`__.
INDI Controller
~~~~~~~~~~~~~~~~
An INDI controller is a controller that immediately processes angle rates to determine
data reliability. This controller can be used together with a traditional PID controller,
which provides a faster angle processing than a string-level PID controller.
For your reference: `Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air
Vehicles <https://arc.aiaa.org/doi/pdf/10.2514/1.G001490>`__.
PID Parameter Tuning
---------------------
**Crazyflie Rate PID is tuned as follows:**
1. Adjust ``Rate`` mode first: set ``rollType``, ``pitchType``, and ``yawType`` to ``RATE``;
2. Adjust ``ATTITUDE`` mode: set the ``KP``, ``KI``, and ``KD`` of ``roll``, ``pitch``, and ``yaw`` to ``0.0``, and only remain the parameters of ``Rate`` unchanged.
3. Adjust ``RATE`` mode: set the ``KI``, ``KD`` of ``roll``, ``pitch`` and ``yaw`` to ``0.0``. Set the ``KP`` first.
4. Burn the code and start the ``KP`` adjustment online using the param function of cfclient.
5. Note that the modified parameters using cfclient will not be saved when power down;
6. During PID tuning, shake (over-tuning) may happen, please be careful.
7. Hold the drone to make sure it can only roll around ``pitch`` axis. Gradually increase the ``KP`` of ``pitch``, till the drone starts shaking back and forth.
8. If the drone shakes intensely, slightly lower ``KP``, generally 5%-10% lower than the shake’s critical point.
9. Tune the ``roll`` and ``yaw`` in the same way.
10. Adjust ``KI`` to eliminate steady-state errors. If only with proportional adjustment, but without this parameter, the drone may swing up and down at Position 0 due to the interference such as gravity. Set the initial value of ``KI`` to 50% of ``KP``.
11. When the ``KI`` increases to certain value, the drone starts shaking. But compared with the shake caused by ``KI``, that caused by ``KP`` is more low-frequency. Keep in mind the point when the drone starts shaking, and mark this ``KI`` as the critical point. The final ``KI`` should be 5%-10% lower than this critical point.
12. Tune the ``roll`` and ``yaw`` in the same way.
13. In general, the value of ``KI`` should be over 80% of the ``KP``.
``Rate PID`` parameter tuning is done now.
**Let’s start the tuning of Attitude PID**
1. First ensure that ``Rate PID`` tuning is completed.
2. Adjust ``rollType``, ``pitchType``, and ``yawType`` to ``ANGLE``\, i.e. the drone is in attitude mode now.
3. Set the ``KI`` and ``KD`` of ``roll`` and ``pitch`` to ``0.0``\, and then set the ``KP``\, \ ``KI``\, and \ ``KD``\ of ``Yaw`` to ``0.0``\.
4. Burn the code and start the ``KP`` tuning online using the param function of cfclient.
5. Set the ``KP`` of ``roll`` and ``pitch`` to ``3.5``\. Check for any existing instability, such as shakes. Keep increasing the KP until the limit is reached;
6. If the ``KP`` already is causing drone instability, or the value is over ``4``, please lower the ``KP`` and ``KI`` of ``RATE`` mode by 5% ~ 10%. By such way, we have more freedom to tune the Attitude mode.
7. If you still need to adjust the KI, please slowly increase KI again. If some low-frequency shakes occur, it indicates that your drone is in an unstable state.
docs/zh_CN/rst/communication.rst
+220 -2
View File
@@ -1,3 +1,221 @@
========
通信协议
========
========
:link_to_translation:`en:[English]`
通信层级结构
------------
====== =================== =======================
终端 手机/PC ESP-Drone
应用层 APP 飞控固件
协议层 CRTP CRTP
传输层 UDP UDP
物理层 Wi-Fi STA (Station) Wi-Fi AP (Access Point)
====== =================== =======================
Wi-Fi 通信
----------
Wi-Fi 性能
~~~~~~~~~~~~
**ESP32 Wi-Fi 性能**
======== =======================================================================
项目 参数
======== =======================================================================
模式 STA 模式、AP 模式、共存模式
协议 IEEE 802.11b、IEEE 802.11g、IEEE 802.11n、802.11 LR（乐鑫）支持软件切换
安全性 WPA、WPA2、WPA2-Enterprise、WPS
主要特性 AMPDU、HT40、QoS
支持距离 乐鑫专属协议下 1 km
传输速率 20 Mbit/s TCP 吞吐量、30 Mbit/s UDP
======== =======================================================================
其它参数见 `ESP32 Wi-Fi 特性列表 <https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-guides/wifi.html#esp32-wi-fi-feature-list>`__\ 。
**ESP32-S2 Wi-Fi 性能**
======== =====================================================
项目 参数
======== =====================================================
模式 STA 模式、AP 模式、共存模式
协议 IEEE 802.11b、IEEE 802.11g、IEEE 802.11n 支持软件切换
安全性 WPA、WPA2、WPA2-Enterprise、WPS
主要特性 AMPDU、HT40、QoS
支持距离 乐鑫专属协议下 1 km
传输速率 20 Mbit/s TCP 吞吐量、30 Mbit/s UDP
======== =====================================================
其他参数见 `ESP32-S2 Wi-Fi 特性列表 <https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/api-guides/wifi.html#esp32-s2-wi-fi-feature-list>`__\ 。
Wi-Fi 编程框架
~~~~~~~~~~~~~~
**基于 ESP-IDF 的 Wi-Fi 编程框架：**
.. figure:: https://img-blog.csdnimg.cn/20200423173923300.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70#pic_center
:align: center
:alt: Wi-Fi 编程模型
:figclass: align-center
Wi-Fi 编程模型
**一般使用过程如下：**
1. 应用层调用 `Wi-Fi 驱动 API <https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_wifi.html>`__\ ，进行 Wi-Fi 初始化。
2. Wi-Fi 驱动对开发人员透明。事件发生，则 Wi-Fi 驱动向默认事件循环：`default event loop <https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/system/esp_event.html#esp-event-default-loops>`__ 发布 ``event``\ 。应用程序可根据需求编写 ``handle`` 程序，进行注册。
3. 网络接口组件 `esp_netif <https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/network/esp_netif.html>`__ 提供了一系列 ``handle`` 程序，与 Wi-Fi 驱动 ``event`` 默认关联。例如 ESP32 作为 AP，当有用户接入时，esp_netif 将自动启动 DHCP 服务。
具体的使用过程，可查阅代码 ``\components\drivers\general\wifi\wifi_esp32.c``。
.. note::
Wi-Fi 初始化之前应使用 ``WIFI_INIT_CONFIG_DEFAULT`` 获取初始化配置结构体，对该结构体进行个性化配置，然后进行初始化工作。请注意防范结构体成员未初始化导致的问题，在 ESP-IDF 更新添加了新的结构体成员时，应尤其特别注意这一问题。
**AP 模式工作状态图：**
.. figure:: https://img-blog.csdnimg.cn/2020042622523887.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70#pic_center
:align: center
:alt: Sample Wi-Fi Event Scenarios in AP Mode
:figclass: align-center
AP 模式下 Wi-Fi Event 示例
提高 Wi-Fi 通信距离
~~~~~~~~~~~~~~~~~~~
依次进入：``Component config>>PHY>>Max WiFi TX power (dBm)``，将 ``Max WiFi TX power`` 改为 ``20``。该项配置将提高 PHY 增益，提高 Wi-Fi 通信距离。
UDP 通信
--------
UDP 端口号
~~~~~~~~~~
=================== ===== ===================
App 方向 ESP-Drone
=================== ===== ===================
192.168.43.42::2399 TX/RX 192.168.43.42::2390
=================== ===== ===================
UDP 包结构
~~~~~~~~~~
.. code:: text
/* Frame format:
* +=============+-----+-----+
* | CRTP | CKSUM |
* +=============+-----+-----+
*/
- UDP 传输的数据包为：CRTP + 校验信息。
- CRTP：按照 CRTP 包结构定义，包含 Header + Data，具体参考 CRTP 协议部分。
- CKSUM：为校验信息，大小为 1 byte，将 CRTP 包按照 byte 累加即可。
**CKSUM 计算方法**
.. code:: python
#python为例：计算 raw 的 cksum，并将其添加到包尾
raw = (pk.header,) + pk.datat
cksum = 0
for i in raw:
cksum += i
cksum %= 256
raw = raw + (cksum,)
CRTP 协议
---------
ESP-Drone 项目继承 Crazyflie 项目使用的 CRTP 协议，用于飞行指令发送、飞行数据回传、参数设置等。
CRTP 实现了无状态设计，不需要握手步骤。任何命令均可在任意时刻发送，但对于一些 log/param/mem 命令，需下载 TOC (目录)，协助主机正确发送信息。已经实现的 Python API (cflib) 实现下载 param/log/mem TOC，确保能够使用所有功能。
CRTP 包结构
~~~~~~~~~~~
CRTP 包大小为 32 字节，其中包含一个字节的 Header，31 个字节的 Payload。Header 记录端口（4 位）、通道（2 位）、及保留位（2 位）。
.. code:: text
7 6 5 4 3 2 1 0
+---+---+---+---+---+---+---+---+
| Port | Res. | Chan. |
+---+---+---+---+---+---+---+---+
| DATA 0 |
+---+---+---+---+---+---+---+---+
: : : : : : : : :
+---+---+---+---+---+---+---+---+
| DATA 30 |
+---+---+---+---+---+---+---+---+
====== ====== ============== ================
字段 字节 位 描述
====== ====== ============== ================
Header 0 0 ~ 1 目标数据通道
\ 0 2 ~ 3 保留，用于传输层
\ 0 4 ~ 7 目标数据端口
Data 1 ~ 31 0 ~ 7 数据包中的数据
====== ====== ============== ================
端口分配
~~~~~~~~
====== ===================== ===================================================================================
端口号 数据端口 用途
====== ===================== ===================================================================================
0 Console Console 使用 consoleprintf 函数将调试信息输出到 PC 端。
2 Parameters 读写 Crazyflie 参数。参数可在源码中用宏表示。
3 Commander 发送 roll/pitch/yaw/thrust 控制指令。
4 Memory access 访问非易失性存储，如 1 线访问和 I2C 访问。仅支持 Crazyflie 2.0
5 Log 设置日志变量。日志变量将定期发送至 Crazyflie，日志变量在 Crazyflie 源码中用宏表示。
6 Localization 本地化相关包
7 Generic Setpoint 运行发送定位点和控制模式
13 Platform 用于 misc platform 控制，如调试和掉电等
14 Client-side debugging 用于调试 PC 端 UI 界面程序，只针对 Crazyflie Python API。
15 Link layer 用于控制和访问通信链路层。
====== ===================== ===================================================================================
固件中大部分连接到端口的模块，以任务的方式实现。如果有传入的 CRTP 包在信息传递队列中传递，则任务在队列中阻塞。启动时，每个任务及其它模块需要在通信链路层为预定义的端口注册。
各个端口使用详情可参考：`CRTP - 与 Crazyflie 通信 <https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/>`__。
CRTP 协议支持包
~~~~~~~~~~~~~~~
cflib 是 CRTP 协议的 Python 支持包，提供了通信协议的应用层接口，可用于构建上位机，与 Crazyflie 和 Crazyflie 2.0 四轴飞行器通信并控制飞行器。固件中每一个使用 CRTP 协议的组件，在 cflib 中都有一个脚本与其对应。
- 源工程仓库地址：`crazyflie-lib-python <https://github.com/bitcraze/crazyflie-lib-python>`__。
- 适配 ESP-Drone 的 cflib 工程仓库地址：`qljz1993/crazyflie-lib-python <https://github.com/qljz1993/crazyflie-lib-python.git>`__。需要切换到 ``esplane`` 分支。
基于 CRTP 协议的应用开发
------------------------
各个平台工程模板
~~~~~~~~~~~~~~~~
1. `crazyflie2-ios-client <https://github.com/bitcraze/crazyflie2-ios-client>`__
2. `crazyflie2-windows-uap-client <https://github.com/bitcraze/crazyflie2-windows-uap-client>`__
3. `crazyflie-android-client <https://github.com/bitcraze/crazyflie-android-client>`__
4. `安卓版本使用指南 <https://wiki.bitcraze.io/doc:crazyflie:client:cfandroid:index>`__
5. `安卓版本开发指南 <https://wiki.bitcraze.io/doc:crazyflie:dev:env:android>`__
cfclient
~~~~~~~~
cfclient 是 ``Crazeflie`` 源工程的上位机，完全实现了 ``CRTP`` 协议中定义的功能，可以加快飞机的调试过程。ESP-Drone 项目对该上位机进行裁剪和调整，满足功能设计需求。
.. figure:: ../../_static/cfclient.png
:align: center
:alt: cfclient控制台界面
:figclass: align-center
cfclient控制台界面
cfclient 具体使用说明可查阅：`cfclient <gettingstarted.html#pc-cfclient>`__。
docs/zh_CN/rst/developerguide.rst
+8 -35
View File
@@ -1,39 +1,12 @@
开发指引
========
:link_to_translation:`en:[English]`
- 环境搭建
.. toctree::
:maxdepth: 2
- ESP-IDF 环境搭建
- ESP32/ESP32-S2 链接脚本修改
- 获取项目源代码
- 驱动程序
- 通用设备
- I2C 总线设备
- SPI 总线设备
- 飞控系统
- 系统启动流程
- 系统任务管理
- 关键任务介绍
- 传感器硬件抽象
- 传感器校准
- 姿态计算
- 控制算法
- 通信协议
- 通信层级结构
- Wi-Fi 通信
- UDP 通信
- CRTP 协议
- 基于 CRTP 协议的应用开发
- 硬件参考
- 已支持硬件列表
- ESP32_S2_Drone_V1_2
- ESPlane_V2_S2
- ESPlane FC V1
环境搭建 <getespidf>
硬件参考 <hardware>
驱动程序 <drivers>
飞控系统 <system>
通信协议 <communication>
docs/zh_CN/rst/drivers.rst
+637 -2
View File
@@ -1,3 +1,638 @@
========
驱动程序
========
========
:link_to_translation:`en:[English]`
本节将主要介绍 ESP-Drone 中使用到的 I2C 驱动和 SPI 驱动。
I2C 设备驱动 (i2c_devices)
--------------------------
I2C 驱动涵盖 MPU6050 传感器驱动和 VL53LXX 传感器驱动。后续部分将从传感器主要特性、关键寄存器和编程注意事项进行介绍。
MPU6050 传感器
~~~~~~~~~~~~~~
概述
^^^^^^
MPU6050 是一款整合性 6 轴运动处理组件，内带 3 轴陀螺仪、3 轴加速度传感器和数字运动处理器（DMP: Digital Motion Processor）。
**工作原理**
- 陀螺仪：当陀螺仪围绕任何感应轴旋转时，由于科里奥利效应，会产生振动。这种振动可以被电容式传感器检测到，传感器所得到的信号被放大，解调和滤波，产生与角速度成比例的电压。
- 电子加速度计：加速沿着一条特定轴在相应的检测质量上位移，则电容式传感器会检测到电容的变化。
**测量范围**
- 可配置陀螺仪测量范围：±250 °/sec、±500 °/sec、±1000 °/sec、±2000 °/sec
- 可配置加速度计测量范围：±2 g、±4 g、±8 g、±16 g
**AUX I2C 接口**
- MPU6050 具有一个辅助 I2C 总线，用于与片外 3-轴数字磁力计或其它传感器进行通信。
- AUX I2C 接口支持两种工作模式：I2C Master 模式或 Pass-Through 模式。
**MPU6050 FIFO**
MPU6050 包含一个可通过串行接口访问的 1024 字节 FIFO 寄存器。FIFO 配置寄存器决定哪个数据写入 FIFO，包括：陀螺仪数据、加速计数据、温度读数、辅助传感器读数和 FSYNC 输入。
**数字低通滤波器 (DLPF)**
MPU6050 自带低通滤波器，可通过配置寄存器 26 控制低通滤波频段，减少高频干扰，但会降低传感器输入速率。开启 DLPF 加速度计输出 1 kHz，关闭 DLPF 可以输出 8 kHz。
**FSYNC 帧同步采样管脚**
寄存器 26 EXT_SYNC_SET，用于配置外部帧同步管脚的采样。
**数字运动处理器 (DMP)**
- MPU6050 内部存在一个数字运动处理单元 (Digital Motion Processor, DMP)，可以计算四元数等，减轻主 CPU 压力。
- DMP 可通过管脚触发中断。
.. figure:: ../../_static/mpu6050_dmp.png
:align: center
:alt: MPU6050 DMP
:figclass: align-center
MPU6050 DMP
**MPU6050 方向定义**
.. figure:: ../../_static/mpu6050_xyz.png
:align: center
:alt: mpu6050_xyz
MPU6050 X、Y、Z 方向
MPU6050 初始化步骤
^^^^^^^^^^^^^^^^^^
1. 恢复寄存器默认值：设置 PWR_MGMT_1 bit7 为 1，恢复后 bit7 为 0，bit6 自动设置为 1，进入 sleep 模式；
2. 设置 PWR_MGMT_1 bit6 为 0，唤醒传感器；
3. 设置时钟源；
4. 设置量程：分别设置陀螺仪和加速度计量程；
5. 设置采样率；
6. 设置数字低通滤波器（可选）。
MPU6050 关键寄存器
^^^^^^^^^^^^^^^^^^
**寄存器典型值**
============ ====== ========================================
寄存器 典型值 功能
============ ====== ========================================
PWR_MGMT_1 0x00 正常启用
SMPLRT_DIV 0x07 陀螺仪采样率 125 Hz
CONFIG 0x06 低通滤波器频率为 5 Hz
GYRO_CONFIG 0x18 陀螺仪不自检，输出满量程范围为 ±2000 °/s
ACCEL_CONFIG 0x01 加速度计不自检，输出的满量程范围为 ±2 g
============ ====== ========================================
**寄存器 117 WHO_AM_I - 设备地址**
[6:1] 保存设备地址，默认为 0x68，不反映 AD0 管脚值。
.. figure:: ../../_static/REG_75.png
:align: center
:alt: WHO_AM_I
:figclass: align-center
**寄存器 107 PWR_MGMT_1 - 电源管理 1**
.. figure:: ../../_static/REG_6B.png
:align: center
:alt: PWR_MGMT_1
:figclass: align-center
- DEVICE_RESET：置位此位，则寄存器使用默认值。
- SLEEP：置位此位，则 MPU6050 将置于睡眠模式。
- CYCLE：置位此位，且 SLEEP 设置为禁用时，MPU6050 将进入循环模式 (CYCLE)。在循环模式下，MPU6050 进入睡眠，达到 LP_WAKE_CTRL（寄存器 108）设定的时间后，从睡眠模式唤醒，完成一次对活动传感器的采样，然后再进入睡眠模式，依次循环。
**寄存器 26 CONFIG - 配置数字低通滤波器**
.. figure:: ../../_static/REG_1A.png
:align: center
:alt: CONFIG
:figclass: align-center
- 数字低通滤波器 (DLPF) 取值与滤波频段关系如下：
.. figure:: ../../_static/DLPF_CFG.png
:align: center
:alt: DLPF
:figclass: align-center
**寄存器 27 - GYRO_CONFIG 陀螺仪量程配置**
.. figure:: ../../_static/REG_1B.png
:align: center
:alt: GYRO_CONFIG
:figclass: align-center
- XG_ST：X 轴陀螺仪自检
- FS_SEL：用于配置陀螺仪量程，具体信息见下表：
.. figure:: ../../_static/FS_SEL.png
:align: center
:alt: FS_SEL
:figclass: align-center
**寄存器 28 ACCEL_CONFIG - 配置加速度计量程**
.. figure:: ../../_static/REG_1C.png
:align: center
:alt: ACCEL_CONFIG
:figclass: align-center
.. figure:: ../../_static/AFS_SEL.png
:align: center
:alt: AFS_SEL
:figclass: align-center
**寄存器 25 SMPRT_DIV - 采样速率分频器**
该寄存器指定陀螺仪输出速率的分频器，用于为 MPU6050 生成采样速率。传感器寄存器的输出、FIFO 输出和 DMP 采样都是基于采样率。陀螺仪输出速率除以 (1 + SMPLRT_DIV) 得到采样率，公式如下：
.. figure:: ../../_static/REG_19.png
:align: center
:alt: SMPRT_DIV
:figclass: align-center
..
Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
其中，当 DLPF 禁用时，即 DLPF_CFG = 0 或 7 时，陀螺仪输出速率为 8 kHz；当 DLPE 使能时，见寄存器 26，陀螺仪输出速率为 1 kHz。注意，在不开启 DLPF 的情况下，设置 SMPLRT_DIV 为 7 可以使芯片产生 1kHz 的中断信号。
.. figure:: ../../_static/mpu6050_int_plot.png
:align: center
:alt: SMPLRT_DIV=7
:figclass: align-center
**寄存器 59 ~ 64 - 加速度计测量值**
.. figure:: ../../_static/REG_3B_40.png
:align: center
:alt: REG_3B_40
:figclass: align-center
- 大端序存放：地址低位存放数据高位，地址高位存放数据低位。
- 补码存放：测量值为有符号整数，因此采用补码方式存放。
**寄存器 65 ~ 66 - 温度测量**
.. figure:: ../../_static/REG_41_42.png
:align: center
:alt: REG_41_42
:figclass: align-center
**寄存器 67 ~ 72 - 陀螺仪测量值**
.. figure:: ../../_static/REG_43_48.png
:align: center
:alt: REG_43_48
:figclass: align-center
VL53LXX 传感器
~~~~~~~~~~~~~~
**概述**
VL53L1X 是 ST 公司提供的一款 ToF 测距和姿态检测传感器。
**工作原理**
VL53L0X/VL53L1X 芯片内部集成了激光发射器和 SPAD 红外接收器。芯片通过探测光子发送和接收时间差，计算光子飞行距离，最远测量距离可达两米，适合中短距离测量的应用。
.. figure:: ../../_static/vl53l1x_package.png
:align: center
:alt: VL53LXX
:figclass: align-center
VL53LXX
**测量区域 - ROI**
VL53L0X/VL53L1X 的测量值为测量区域中的最短距离，测量区域可以根据使用场景进行放大或缩小，较大的探测范围可能会引起测量值的波动。
测量区域的配置参见 `VL53LXX Datasheet <https://www.st.com/resource/en/datasheet/vl53l1x.pdf>`__ 中 2.4 Ranging Description 和 2.8 Sensing Array Optical Center。
.. figure:: ../../_static/vl53lxx_roi.png
:align: center
:alt: ROI
:figclass: align-center
ROI
**测量距离**
- VL53L0X 传感器存在 **3 ~ 4 cm 的盲区**\ ，有效测量范围为 3 ~ 200 cm，精度 ±3%。
- VL53L1X 是 VL53L0X 的升级版本，探测距离可达 400 cm。
.. figure:: ../../_static/vl53lxx_mode.png
:align: center
:alt: VL53L0X mode
:figclass: align-center
- VL53LXX 测量距离与光线环境有关，黑暗环境下探测距离更远；在室外强光下，激光传感器可能会受到很大的干扰，导致测量精度降低，因此室外需要结合气压定高。
.. figure:: ../../_static/vl53l1x_max_distance.png
:align: center
:alt: VL53L1X mode
:figclass: align-center
**测量频率**
- VL53L0X 响应频率最快可达 50 Hz，测量误差 ±5%。
- VL53L1X I2C 最高时钟频率可达 400 kHz，上拉电阻需要根据电压和总线电容值选择。具体信息请参考 `VL53LXX Datasheet <https://www.st.com/resource/en/datasheet/vl53l1x.pdf>`__\ 。
.. figure:: ../../_static/vl53l1x_typical_circuit.png
:align: center
:alt: vl53l1x
:figclass: align-center
VL53L1X
- XSHUT 为输入管脚，用于模式选择（休眠），需要上拉电阻防止漏电流。
- GPIO1 为中断输出管脚，用于输出测量 dataready 中断。
**工作模式**
通过设置 XSHUT 管脚的电平，可以切换传感器进入 HW Standby 模式或 SW Standby 模式，实现有条件启动，降低待机功耗。如果主机放弃管理传感器模式，可将 XSHUT 管脚默认设置为上拉。
- HW Standby：XSHUT 拉低，传感器电源关闭。
- SW Standby：XSHUT 拉高，进入 boot 和 SW Standby 模式。
.. figure:: ../../_static/vl53lxx_power_up_sequence.png
:align: center
:alt: HW Standby
:figclass: align-center
HW Standby
.. figure:: ../../_static/vl53lxx_boot_sequence.png
:align: center
:alt: SW Standby
:figclass: align-center
SW Standby
VL53LXX 初始化步骤
^^^^^^^^^^^^^^^^^^
1. 等待硬件初始化完成；
2. 数据初始化；
3. 静态初始化，装载数据；
4. 设置测量距离模式；
5. 设置单次测量最长等待时间；
6. 设置测量频率（时间间隔）；
7. 设置测量区域 ROI（可选）；
8. 启动测量。
.. code:: text
/*init vl53l1 module*/
void vl53l1_init()
{
Roi0.TopLeftX = 0; //测量目标区（可选）最小 4*4，最大 16*16。
Roi0.TopLeftY = 15;
Roi0.BotRightX = 7;
Roi0.BotRightY = 0;
Roi1.TopLeftX = 8;
Roi1.TopLeftY = 15;
Roi1.BotRightX = 15;
Roi1.BotRightY = 0;
int status = VL53L1_WaitDeviceBooted(Dev); //等待硬件初始化完成。
status = VL53L1_DataInit(Dev); //数据初始化，上电后立刻执行。
status = VL53L1_StaticInit(Dev); //静态初始化，装载参数。
status = VL53L1_SetDistanceMode(Dev, VL53L1_DISTANCEMODE_LONG);//设置测量模式。
status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(Dev, 50000); //根据测量模式确定最长等待时间。
status = VL53L1_SetInterMeasurementPeriodMilliSeconds(Dev, 100); //设置测量间隔。
status = VL53L1_SetUserROI(Dev, &Roi0); //设置测量区域 ROI
status = VL53L1_StartMeasurement(Dev); //启动测量
if(status) {
printf("VL53L1_StartMeasurement failed \n");
while(1);
}
}
注意，上述初始化步骤除 VL53L1_SetUserROI 外，其余步骤不可缺少。
VL53LXX 测距步骤
^^^^^^^^^^^^^^^^
**轮询测量模式**
轮询测量流程图：
.. figure:: ../../_static/vl53lxx_meaturement_sequence.png
:align: center
:alt: vl53lxx_meaturement_sequence
:figclass: align-center
VL53LXX 测量流程
- 注意，完成一次测量和读取后，需要使用 ``VL53L1_ClearInterruptAndStartMeasurement`` 清除中断标志并重新开始。
- 轮询测量有两种方法，如上图所示：一种是阻塞方式 (Drivers polling mode)；一种是非阻塞方式 (Host polling mode)。以下代码为阻塞测量方式：
.. code:: text
/* Autonomous ranging loop*/
static void
AutonomousLowPowerRangingTest(void)
{
printf("Autonomous Ranging Test\n");
static VL53L1_RangingMeasurementData_t RangingData;
VL53L1_UserRoi_t Roi1;
int roi = 0;
float left = 0, right = 0;
if (0/*isInterrupt*/) {
} else {
do // polling mode
{
int status = VL53L1_WaitMeasurementDataReady(Dev); //等待测量结果
if(!status) {
status = VL53L1_GetRangingMeasurementData(Dev, &RangingData); //获取单次测量数据
if(status==0) {
if (roi & 1) {
left = RangingData.RangeMilliMeter;
printf("L %3.1f R %3.1f\n", right/10.0, left/10.0);
} else
right = RangingData.RangeMilliMeter;
}
if (++roi & 1) {
status = VL53L1_SetUserROI(Dev, &Roi1);
} else {
status = VL53L1_SetUserROI(Dev, &Roi0);
}
status = VL53L1_ClearInterruptAndStartMeasurement(Dev); //释放中断
}
}
while (1);
}
// return status;
}
**中断测量模式**
中断测量模式需要使用中断管脚 GPIO1，在数据 ready 时，GPIO1 管脚电平将被拉低，通知主机读取数据。
.. figure:: ../../_static/vl53lxx_sequence.png
:align: center
:alt: vl53lxx autonomous sequence
:figclass: align-center
VL53LXX 自主测量流程
VL53LXX 传感器校准
^^^^^^^^^^^^^^^^^^
如果传感器接收器上方安装了光罩，或传感器安装在透明盖板背后，由于透光率的变化，需要对传感器进行校准。您可以根据校准流程调用 API 编写校准程序，也可以使用官方提供的 GUI 上位机直接测量出校准值。
**使用官方 API 编写校准程序**
校准流程：调用顺序要完全一致。
.. figure:: ../../_static/vl53lxx_calibration_sequence.png
:align: center
:alt: vl53lxx_calibration_sequence
:figclass: align-center
VL53LXX 校准流程
::
/*VL53L1 模块校准*/
static VL53L1_CalibrationData_t vl53l1_calibration(VL53L1_Dev_t *dev)
{
int status;
int32_t targetDistanceMilliMeter = 703;
VL53L1_CalibrationData_t calibrationData;
status = VL53L1_WaitDeviceBooted(dev);
status = VL53L1_DataInit(dev); //设备初始化
status = VL53L1_StaticInit(dev); // 为给定用例，加载设备设置。
status = VL53L1_SetPresetMode(dev,VL53L1_PRESETMODE_AUTONOMOUS);
status = VL53L1_PerformRefSpadManagement(dev);
status = VL53L1_PerformOffsetCalibration(dev,targetDistanceMilliMeter);
status = VL53L1_PerformSingleTargetXTalkCalibration(dev,targetDistanceMilliMeter);
status = VL53L1_GetCalibrationData(dev,&calibrationData);
if (status)
{
ESP_LOGE(TAG, "vl53l1_calibration failed \n");
calibrationData.struct_version = 0;
return calibrationData;
}else
{
ESP_LOGI(TAG, "vl53l1_calibration done ! version = %u \n",calibrationData.struct_version);
return calibrationData;
}
}
**使用官方 GUI 上位机校准传感器**
官方提供了用于配置和校准传感器的 GUI 上位机，配合 ST 官方 ``STM32F401RE nucleo`` 开发板连接传感器，使用软件校准得到基准值后，初始化时填入即可。
.. figure:: ../../_static/vl53lxx_calibrate_gui.png
:align: center
:alt: STSW-IMG008
:figclass: align-center
STSW-IMG008
更多信息，可参考 `STSW-IMG008: Windows Graphical User Interface (GUI) for VL53L1X Nucleo packs. Works with P-NUCLEO-53L1A1 <https://www.st.com/content/st_com/en/products/embedded-software/proximity-sensors-software/stsw-img008.html>`__\ 。
VL53L1X 例程
^^^^^^^^^^^^
**例程说明**
1. 实现功能：通过 VL53L1X 检测到高度变化（持续 1 秒），红灯亮起。高度恢复正常值（持续 1 秒），绿灯亮起。
2. 可配置参数：通过 ``make menuconfig`` 设置 I2C 号码、端口号、LED 端口号。
3. 例程解析见代码注释和用户手册。
**注意事项**
1. 该例程只适用于 VL53L1X，VL53L0X 为老版本硬件，不适用本例程。
2. 官方标称 400 cm 测量距离，为黑暗环境下测得。室内正常灯光环境，可以保证 10 ~ 260 cm 范围的有效测量。
3. 初始化函数 ``vl53l1\_init (VL53L1\_Dev\_t \*)`` 中部分参数，需要根据实际使用环境确定，还有优化的空间。
4. 传感器安装位置应确保在检测位置正上方。
5. 模块上电时自动矫正基准高度，如果基准高度有变化，需要重新上电重置参数。
**例程仓库**
点击 `esp32-vl53l1x-test <https://github.com/qljz1993/esp32-vl53l1x-test/tree/master>`__ 查看例程，或使用 git 工具下载例程：
.. code:: text
git clone https://github.com/qljz1993/esp32-vl53l1x-test.git
SPI 设备驱动 (spi_devices)
--------------------------
PMW3901 传感器
~~~~~~~~~~~~~~
PMW3901 是 PixArt 公司最新的高精度低功耗光学追踪模组，可直接获取 X-Y 方向运动速度信息，实现对地高度 8 cm 以上进行有效测量。PWM3901
工作电流小于 9 mA，工作电压为 VDD (1.8 ~ 2.1 V)，VDDIO (1.8 ~ 3.6 V)，使用 4 线 SPI 接口通信。
**主要参数**
============= =======================================
参数 参数值
============= =======================================
供电电压 (V) VDD：1.8 ~ 2.1 V；VDDIO：1.8 ~ 3.6 V
工作范围 (mm) 80 ~ +∞
接口 4 线 SPI @ 2 MHz
封装 28 管脚 COB 封装，尺寸：6 x 6 x 2.28 mm
============= =======================================
**封装和管脚图**
.. figure:: ../../_static/pmw3901_package.png
:align: center
:alt: pmw3901_package
:figclass: align-center
PMW3901 封装
.. figure:: ../../_static/pmw3901_pinmap.png
:align: center
:alt: pmw3901_pinmap
:figclass: align-center
PMW3901 管脚映射
传感器工作电压较低，与 3.3 V 的 ESP32 通信，需要 VDD 和 VDDIO 提供不同的电压。
上电启动流程
^^^^^^^^^^^^
**上电流程**
PMW3901MB 虽然可执行内部上电自复位，但仍建议每次上电时，对 Power_Up_Reset 寄存器执行写操作。具体顺序如下:
1. 首先对 VDDIO 供电，然后对 VDD 供电，中间延迟不应超过 100 ms。注意确保供电稳定。
2. 等待至少 40 ms。
3. 先拉高，然后再拉低 NCS，以复位 SPI 口。
4. 写 0x5A 到 Power_Up_Reset 寄存器，或切换至 NRESET 管脚。
5. 等待至少 1 ms。
6. 无论运动管脚状态如何，一次读取寄存器 0x02、0x03、0x04、0x05 和 0x06。
7. 请参考 PWM3901MB Datasheet 章节 8.2 性能优化寄存器来配置所需的寄存器，以实现芯片的最佳性能。
**掉电流程**
通过写 Shutdown 寄存器，可将 PMW3901MB 设置为 Shutdown 模式。Shutdown 模式下 PMW3901MB 只对上电启动指令（写 0x5A 到寄存器 0x3A）进行响应，不响应其它访问操作。同一 SPI 总线下的其它设备不受 PMW3901MB Shutdown 模式的影响，在 NCS 管脚不冲突的情况下可以正常访问。
从 Shutdown 模式复位：
1. 拉高，然后再拉低 NCS，以复位 SPI 口；
2. 写 0x5A 到 Power_Up_Reset 寄存器，或切换至 NRESET 管脚；
3. 等待至少 1 ms；
4. 无论运动管脚状态如何，一次性读取寄存器 0x02、0x03、0x04、0x05 和 0x06；
5. 请参考 PWM3901MB Datasheet 章节 8.2 性能优化寄存器来配置所需的寄存器，以实现芯片的最佳性能。
更多信息见 `PixArt 其它产品助力 IoT <https://www.pixart.com/applications/11/Connected_Home_Appliances_%EF%BC%86_IoT>`__。
部分代码解读
^^^^^^^^^^^^
**关键结构体**
::
typedef struct opFlow_s
{
float pixSum[2]; /*累积像素*/
float pixComp[2]; /*像素补偿*/
float pixValid[2]; /*有效像素*/
float pixValidLast[2]; /*上一次有效像素*/
float deltaPos[2]; /*2 帧之间的位移 单位：cm*/
float deltaVel[2]; /*速度 单位 cm/s*/
float posSum[2]; /*累积位移 单位 cm*/
float velLpf[2]; /*速度低通 单位 cm/s*/
bool isOpFlowOk; /*光流状态*/
bool isDataValid; /*数据有效*/
} opFlow_t;
- 累积像素：飞行器起飞后的累积像素；
- 像素补偿：补偿由于飞行器倾斜导致的像素误差；
- 有效像素：指经过补偿的实际像素；
- 2 帧之间的位移：即由像素转换出来的实际位移，单位 cm；
- 速度：表示瞬时速度，由位移变化量微分得到，单位 cm/s；
- 累积位移：实际位移，单位 cm；
- 速度低通：对速度进行低通，增加数据平滑性；
- 光流状态：检查光流是否正常工作；
- 数据有效：在一定高度范围内，数据有效。
::
typedef struct motionBurst_s {
union {
uint8_t motion;
struct {
uint8_t frameFrom0 : 1;
uint8_t runMode : 2;
uint8_t reserved1 : 1;
uint8_t rawFrom0 : 1;
uint8_t reserved2 : 2;
uint8_t motionOccured : 1;
};
};
uint8_t observation;
int16_t deltaX;
int16_t deltaY;
uint8_t squal;
uint8_t rawDataSum;
uint8_t maxRawData;
uint8_t minRawData;
uint16_t shutter;
} __attribute__((packed)) motionBurst_t;
- motion：运动信息，可以根据不同的位去判断运动信息，包括帧判别、运行模式和运动信息检测等；
- observation：用于检测 IC 是否出现 EFT/B 或 ESD 问题。传感器正常工作时，读取出来的值为 0xBF；
- deltaX 和 deltaY：光流检测到图像 X 和 Y 方向的运动信息；
- squal：指运动信息质量，即运动信息的可信度；
- rawDataSum：原数据求和，可用于对一帧数据求平均值；
- maxRawData 和 minRawData：最大和最小原始数据；
- shutter：是一个实时自动调整的值，确保平均运动数据保持在正常可操作范围以内。shutter 可搭配 squal，用来判断运动信息是否可用。
编程注意事项
^^^^^^^^^^^^
- 如果连续 1 s 内光流数据都为 0，说明出现故障，需要做挂起光流任务等处理；
- 注意，传感器镜头必须朝下安装。由于地方位置固定，根据相对运动，\ **传感器采集的位移数据与飞机实际运动方向相反**\ ；
- 注意，只有在定高模式测试稳定时，才能进入定点模式。精确的高度信息，用于确定图像像素和实际距离的对应关系；
- 手动测试倾角补偿，实现通过补偿使飞行器有一定的倾角时，传感器输出基本不变化；
- 有了倾角补偿和运动累积像素即可以得到实际累积像素。通过相关计算可以得到：
- 2 帧之间变化像素 = 实际积累像素 - 上次实际像素；
- 2 帧之间的位移变化 = 2 帧之间变化像素 x 系数。对系数的限制：高度小于 5 cm，光流即无法工作，所以系数设置为 0；
- 对上述位移积分，得到四轴到起飞点的位移；对上述位移微分，得到瞬时速度。注意对速度进行低通增加数据的平滑性，对速度进行限幅处理，增加数据安全性。
- 通过光流就得到了四轴的位置信息和速度信息，然后：
- 上述位置信息和速度信息融合加速计 (state_estimator.c)，即可得到估测位置和速度；
- 估测位置和速度参与 PID 运算，即可用于水平方向位置控制。请参考 position_pid.c，查看位置环和速度环 PID 的处理过程。
通过上述过程即可实现水平定点控制。
docs/zh_CN/rst/getespidf.rst
+4 -3
View File
@@ -1,5 +1,6 @@
搭建开发环境
============
:link_to_translation:`en:[English]`
ESP-IDF 环境搭建
----------------
@@ -50,9 +51,9 @@ ESP32/ESP32-S2 链接脚本修改
**代码文件结构如下所示：**
.. figure:: ../../_static/espdrone_file_structure.png
:align: center
:alt: espdrone_file_structure
espdrone_file_structure
:figclass: align-center
::
@@ -158,7 +159,7 @@ ESP32/ESP32-S2 链接脚本修改
uint16_t channelAux[MAX_AUX_RC_CHANNELS];
} __attribute__((packed));
**attribute** ((packed)) 的作用是：使编译器取消结构在编译过程中的优化对齐，而按照实际占用字节数进行对齐。这是 GCC 特有的语法，与操作系统无关，与编译器有关。GCC 和 VC（在 Windows 下）的编译器为非紧凑模式，TC 的编译器为紧凑模式。例如：
``__attribute__((packed))`` 的作用是：使编译器取消结构在编译过程中的优化对齐，而按照实际占用字节数进行对齐。这是 GCC 特有的语法，与操作系统无关，与编译器有关。GCC 和 VC（在 Windows 下）的编译器为非紧凑模式，TC 的编译器为紧凑模式。例如：
.. code:: text
docs/zh_CN/rst/gettingstarted.rst
+17 -24
View File
@@ -7,8 +7,7 @@
项目简介
========
ESP-Drone 是基于乐鑫 ESP32/ESP32-S2 开发的小型无人机解决方案，可使用手机
APP 或游戏手柄通过 Wi-Fi
ESP-Drone 是基于乐鑫 ESP32/ESP32-S2 开发的小型无人机解决方案，可使用手机 APP 或游戏手柄通过 Wi-Fi
网络进行连接和控制。该方案硬件结构简单，代码架构清晰，支持功能扩展，可用于
STEAM 教育等领域。项目部分代码来自 Crazyflie 开源工程，继承 GPL3.0
开源协议。
@@ -28,8 +27,7 @@ ESP-Drone 具备以下特性：
- 支持自稳定模式 (Stabilize mode)：自动控制机身水平，保持平稳飞行。
- 支持定高模式 (Height-hold mode)：自动控制油门输出，保持固定高度。
- 支持定点模式 (Position-hold
mode)：自动控制机身角度，保持固定空间位置。
- 支持定点模式 (Position-hold mode)：自动控制机身角度，保持固定空间位置。
- 支持 PC 上位机调试：使用 cfclient 上位机进行静态/动态调试。
- 支持 APP 控制：使用手机 APP 通过 Wi-Fi 轻松控制。
- 支持游戏手柄 (gamepad) 控制：通过 cfclient 使用游戏手柄轻松控制。
@@ -59,13 +57,10 @@ ESP-IDF 简介
ESP-IDF 是乐鑫为 ESP32/ESP32-S2 提供的物联网开发框架。
- ESP-IDF 包含一系列库及头文件，提供了基于 ESP32/ESP32-S2
构建软件项目所需的核心组件。
- ESP-IDF
还提供了开发和量产过程中最常用的工具及功能，例如：构建、烧录、调试和测量等。
- ESP-IDF 包含一系列库及头文件，提供了基于 ESP32/ESP32-S2 构建软件项目所需的核心组件。
- ESP-IDF 还提供了开发和量产过程中最常用的工具及功能，例如：构建、烧录、调试和测量等。
详情可查阅：`ESP-IDF
编程指南 <https://docs.espressif.com/projects/esp-idf/zh_CN/latest/esp32s2/get-started/index.html>`__。
详情可查阅：`ESP-IDF 编程指南 <https://docs.espressif.com/projects/esp-idf/zh_CN/latest/esp32s2/get-started/index.html>`__。
Crazyflie 简介
================
@@ -73,10 +68,8 @@ Crazyflie 简介
Crazyflie 是来自 Bitcraze 开源工程的四旋翼飞行器，具备以下特性：
- 支持多种传感器组合，可以轻松实现定高模式、定点模式等高级飞行模式。
- 基于 FreeRTOS
编写，将复杂的无人机系统，分解成多个具有不同优先级的软件任务。
- 设计了功能完备的 cfclient 上位机和 CRTP
通信协议，便于实现调试、测量和控制。
- 基于 FreeRTOS 编写，将复杂的无人机系统，分解成多个具有不同优先级的软件任务。
- 设计了功能完备的 cfclient 上位机和 CRTP 通信协议，便于实现调试、测量和控制。
.. figure:: ../../_static/crazyflie-overview.png
:align: center
@@ -89,7 +82,7 @@ Crazyflie 是来自 Bitcraze 开源工程的四旋翼飞行器，具备以下特
Delft) <https://img-blog.csdnimg.cn/20191030202634944.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70>`__
详情可查阅 `Crazyflie 官网 <https://www.bitcraze.io/>`__\ 。
详情可查阅 `Crazyflie 官网 <https://www.bitcraze.io/>`__。
准备工作
================
@@ -106,7 +99,7 @@ Crazyflie 是来自 Bitcraze 开源工程的四旋翼飞行器，具备以下特
ESP32-S2-Drone V1.2 组装流程
硬件介绍和管脚资源分配可查阅：\ `硬件参考 <./hardware.rst>`__\ 。
硬件介绍和管脚资源分配可查阅：`硬件参考 <./hardware.rst>`__。
安装 ESP-Drone APP
--------------------
@@ -315,7 +308,7 @@ File <https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1. 最大倾角 (Max angle)：设置最大允许的俯仰和翻滚角度：roll/pitch。
2. 最大自选速度 (Max yaw rate)：设置允许的偏航速度：yaw。
2. 最大自旋速度 (Max yaw rate)：设置允许的偏航速度：yaw。
3. 最大油门 (Max thrust)：设置最大油门。
4. 最小油门 (Min thrust)：设置最小油门。
5. 压摆极限 (Slew limit)：防止油门骤降，油门低于该值时，下降速度将被限定。
@@ -406,11 +399,11 @@ File <https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/
起飞前检查
================
- 将飞机头部朝前放置，尾部天线朝向自己。
- 将飞机置于水平面上，待机身稳定时上电。
- 观察上位机水平面是否置平。
- 观察通信建立以后，小飞机尾部绿灯是否快速闪烁。
- 观察小飞机头部红灯是否熄灭，亮起代表电量不足。
- 轻推左手小油门，检查飞机是否能快速响应。
- 轻推右手方向，检查方向控制是否正确。
- 将小飞机头部朝前放置，尾部天线朝向自己；
- 将小飞机置于水平面上，待机身稳定时上电；
- 观察上位机水平面是否置平；
- 观察通信建立以后，小飞机尾部绿灯是否快速闪烁；
- 观察小飞机头部红灯是否熄灭，亮起代表电量不足；
- 轻推左手小油门，检查飞机是否能快速响应；
- 轻推右手方向，检查方向控制是否正确；
- 起飞吧！
docs/zh_CN/rst/hardware.rst
+30 -23
View File
@@ -1,5 +1,6 @@
硬件参考
========
:link_to_translation:`en:[English]`
已支持硬件
----------
@@ -18,42 +19,41 @@ ESPlane-FC-V1 ESP32-WROOM-32D + MPU6050 需安装机架
硬件切换方法
~~~~~~~~~~~~
- ``esp_drone`` 仓库代码已支持多种硬件，可通过 ``menuconfig`` 进行切换。
- ``esp_drone`` 仓库代码已支持多种硬件，可通过 ``menuconfig`` 进行切换，见下图。
.. figure:: ../../_static/board_choose.png
:align: center
:alt: ESP-Drone config
:figclass: align-center
ESP-Drone config
- 默认情况下，``set-target`` 设为 ``esp32s2`` 后，硬件自动切换为 ``ESP32_S2_Drone_V1_2``。
- 默认情况下，\ ``set-target`` 设为 ``esp32s2``\ 后，硬件自动切换为 ``ESP32_S2_Drone_V1_2``\ 。
- 默认情况下，\ ``set-target`` 设为 ``esp32`` 后，硬件自动切换为 ``ESPlane_FC_V1``\ 。
- 默认情况下，``set-target`` 设为 ``esp32`` 后，硬件自动切换为 ``ESPlane_FC_V1``。
**注意事项**
1. ESPlane-FC-V1 为老版本硬件。
2. ESPlane-FC-V1 使用 ESP-Drone 新版本代码，需要对硬件进行改动，即使用跳线，将模组 GPIO14 连接到 mpu6050 int 管脚。
2. ESPlane-FC-V1 使用 ESP-Drone 新版本代码，需要对硬件进行改动，即使用跳线，将模组 GPIO14 连接到 MPU6050 INT 管脚。
3. ESPlane-FC-V1 防止上电时 IO12 触发 flash 电压切换，使用 ``espefuse.py`` 将 flash 电压固定到 3.3 V：
``espefuse.py --port /dev/ttyUSB0 set_flash_voltage 3.3V``
``note * Only the first device attaching to the bus can use CS0 pin.``
注意，仅有第一个连接到总线的设备可以使用 CS0 管脚。
ESP32-S2-Drone V1.2
-------------------
.. figure:: ../../_static/espdrone_s2_v1_2_up2.jpg
:align: center
:alt: ESP-Drone
:figclass: align-center
ESP-Drone
ESP32-S2-Drone V1.2 外观图
主板原理图：\ `SCH_Mainboard_ESP32_S2_Drone_V1_2 <./_static/ESP32_S2_Drone_V1_2/SCH_Mainboard_ESP32_S2_Drone_V1_2.pdf>`__
主板 PCB：\ `PCB_Mainboard_ESP32_S2_Drone_V1_2 <./_static/ESP32_S2_Drone_V1_2/PCB_Mainboard_ESP32_S2_Drone_V1_2.pdf>`__
- 主板原理图：`SCH_Mainboard_ESP32_S2_Drone_V1_2 <./_static/ESP32_S2_Drone_V1_2/SCH_Mainboard_ESP32_S2_Drone_V1_2.pdf>`__
- 主板 PCB：`PCB_Mainboard_ESP32_S2_Drone_V1_2 <./_static/ESP32_S2_Drone_V1_2/PCB_Mainboard_ESP32_S2_Drone_V1_2.pdf>`__
基础配置
~~~~~~~~
@@ -62,9 +62,11 @@ ESP32-S2-Drone V1.2
^^^^^^^^^^^^
.. figure:: ../../_static/espdrone_s2_v1_2_hardware_package.png
:align: center
:alt: ESP-Drone
:figclass: align-center
ESP-Drone
ESP32-S2-Drone V1.2 基础配置清单
================ ======== ==============================
基础配置清单 数量 备注
@@ -79,7 +81,9 @@ ESP32-S2-Drone V1.2
8-pin 25 mm 排针 2
================ ======== ==============================
注意：更换 720 电机之后，需要在 ``menuconfig->ESPDrone Config->motors config`` 将 ``motor type`` 修改为 ``brushed 720 motor``
.. note::
更换 720 电机之后，需要在 ``menuconfig->ESPDrone Config->motors config`` 将 ``motor type`` 修改为 ``brushed 720 motor``。
主控制器
^^^^^^^^
@@ -186,9 +190,9 @@ GPIO46 CAM_Y3
| -指南针模块 | 罗盘 | 等高级模式 | MPU6050从机 | |
+-------------+-------------+-------------+-------------+-------------+
扩展板原理图：待发布
.. 扩展板原理图：待发布
扩展板 PCB：待发布
.. 扩展板 PCB：待发布
扩展板 IO 定义
^^^^^^^^^^^^^^
@@ -210,25 +214,28 @@ ESPlane-V2-S2
-------------
.. figure:: ../../_static/esplane_2_0.jpg
:align: center
:alt: esplane_fc_v1
:figclass: align-center
esplane_fc_v1
ESPlane-V2-S2 外观图
主板原理图：\ `SCH_ESPlane_V2_S2 <./_static/ESPlane_V2_S2/SCH_ESPlane_V2_S2.pdf>`__
主板 PCB：\ `PCB_ESPlane_V2_S2 <./_static/ESPlane_V2_S2/PCB_ESPlane_V2_S2.pdf>`__
- 主板原理图：`SCH_ESPlane_V2_S2 <./_static/ESPlane_V2_S2/SCH_ESPlane_V2_S2.pdf>`__
- 主板 PCB：`PCB_ESPlane_V2_S2 <./_static/ESPlane_V2_S2/PCB_ESPlane_V2_S2.pdf>`__
ESPlane-FC-V1
-------------
.. figure:: ../../_static/esplane_1_0.jpg
:align: center
:alt: esplane_fc_v1
:figclass: align-center
esplane_fc_v1
ESPlane-FC-V1 外观图
主板原理图：\ `Schematic_ESPlane_FC_V1 <./_static/ESPlane_FC_V1/Schematic_ESPlane_FC_V1.pdf>`__
主板 PCB：\ `PCB_ESPlane_FC_V1 <./_static/ESPlane_FC_V1/PCB_ESPlane_FC_V1.pdf>`__
- 主板原理图：`Schematic_ESPlane_FC_V1 <./_static/ESPlane_FC_V1/Schematic_ESPlane_FC_V1.pdf>`__
- 主板 PCB：`PCB_ESPlane_FC_V1 <./_static/ESPlane_FC_V1/PCB_ESPlane_FC_V1.pdf>`__
.. _Basic_Component-1:
docs/zh_CN/rst/index.rst
+1 -1
View File
@@ -1,6 +1,6 @@
ESP-Drone
===========
:link_to_translation:`en:[English]`
ESP-Drone 是基于乐鑫 ESP32/ESP32-S2 开发的小型无人机解决方案，可使用手机 APP 或游戏手柄通过 Wi-Fi 网络进行连接和控制。目前已支持自稳定飞行、定高飞行、定点飞行等多种模式。该方案硬件结构简单，代码架构清晰完善，方便功能扩展，可用于 STEAM 教育等领域。控制系统代码来自 Crazyflie 开源工程，使用 GPL3.0 开源协议。
docs/zh_CN/rst/notice.rst
+20 -3
View File
@@ -1,3 +1,20 @@
================
Notice
================
第三方代码
==========
:link_to_translation:`en:[English]`
第三方代码及证书如下：
============== ======= ======================================================================================= ========================================
组件 License 源代码 Commit ID
============== ======= ======================================================================================= ========================================
core/crazyflie GPL-3.0 `Crazyflie <https://github.com/bitcraze/crazyflie-firmware>`__ a2a26abd53a5f328374877bfbcb7b25ed38d8111
lib/dsp_lib `esp32-lin <https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib>`__ 6fa39f4cd5f7782b3a2a052767f0fb06be2378ff
============== ======= ======================================================================================= ========================================
致谢
====
1. 感谢 Bitcraze 开源组织提供很棒的 `Crazyflie <https://www.bitcraze.io/%20>`__ 无人机项目代码；
2. 感谢乐鑫提供 ESP32 和 `ESP-IDF 操作系统 <https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html>`__\ ；
3. 感谢 WhyEngineer 提供的 dsp 移植库 `esp-dsp <https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib>`__\ 。
docs/zh_CN/rst/system.rst
+655 -2
View File
@@ -1,3 +1,656 @@
=========
飞控系统
=========
=========
:link_to_translation:`en:[English]`
系统启动流程
------------
.. figure:: ../../_static/start_from_app_main.png
:align: center
:alt: start_from_app_main
:figclass: align-center
源文件见：`start_from_app_main <./_static/start_from_app_main.pdf>`__。
系统任务管理
------------
系统任务简介
~~~~~~~~~~~~
系统正常运行时，将启动以下 TASK。
.. figure:: ../../_static/task_dump.png
:align: center
:alt: task_dump
:figclass: align-center
其中，
* Load：CPU 占用率
* Stack Left：剩余堆栈空间
* Name：TASK 名称
* PRI：TASK 优先级
TASK 具体描述如下：
- PWRMGNT：系统电压监测
- CMDHL：应用层-处理根据 CRTP 协议构成的高级命令
- CRTP-RX：协议层-CRTP 飞行协议解码
- CRTP-TX：协议层-CRTP 飞行协议解码
- UDP-RX：传输层-UDP 包接收
- UDP-TX：传输层-UDP 包发送
- WIFILINK：对接 CRTP 协议层和 UDP 传输层
- SENSORS：传感器数据读取和预处理
- KALMAN：使用传感器数据进行飞机状态估计，包括飞机角度、角速度、空间位置的估计。该
TASK 在 ESP 芯片上 CPU 资源消耗较大，应注意优先级的分配。
- PARAM：使用 CRTP 协议远程修改变量
- LOG：使用 CRTP 协议实时监控变量
- MEM：使用 CRTP 协议远程修改存储器
- STABILIZER：自稳定线程，控制飞控程序运行流程
- SYSTEM：控制系统初始化和自检流程
任务堆栈空间配置
~~~~~~~~~~~~~~~~
用户可以在 ``components/config/include/config.h``
中直接修改空间大小，也可以在 ``menucfg`` 中修改 ``BASE_STACK_SIZE``
大小。使用 ``ESP32`` 时可将 ``BASE_STACK_SIZE`` 调整为
2048，减小踩内存的概率；使用 ``ESP32-S2`` 时，建议将该值调整为
``1024``。
::
//任务堆栈空间大小
#define SYSTEM_TASK_STACKSIZE (4* configBASE_STACK_SIZE)
#define ADC_TASK_STACKSIZE configBASE_STACK_SIZE
#define PM_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define CRTP_TX_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define CRTP_RX_TASK_STACKSIZE (2* configBASE_STACK_SIZE)
#define CRTP_RXTX_TASK_STACKSIZE configBASE_STACK_SIZE
#define LOG_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define MEM_TASK_STACKSIZE (1 * configBASE_STACK_SIZE)
#define PARAM_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define SENSORS_TASK_STACKSIZE (2 * configBASE_STACK_SIZE)
#define STABILIZER_TASK_STACKSIZE (2 * configBASE_STACK_SIZE)
#define NRF24LINK_TASK_STACKSIZE configBASE_STACK_SIZE
#define ESKYLINK_TASK_STACKSIZE configBASE_STACK_SIZE
#define SYSLINK_TASK_STACKSIZE configBASE_STACK_SIZE
#define USBLINK_TASK_STACKSIZE configBASE_STACK_SIZE
#define WIFILINK_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define UDP_TX_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define UDP_RX_TASK_STACKSIZE (2*configBASE_STACK_SIZE)
#define UDP_RX2_TASK_STACKSIZE (1*configBASE_STACK_SIZE)
#define PROXIMITY_TASK_STACKSIZE configBASE_STACK_SIZE
#define EXTRX_TASK_STACKSIZE configBASE_STACK_SIZE
#define UART_RX_TASK_STACKSIZE configBASE_STACK_SIZE
#define ZRANGER_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define ZRANGER2_TASK_STACKSIZE (2* configBASE_STACK_SIZE)
#define FLOW_TASK_STACKSIZE (2* configBASE_STACK_SIZE)
#define USDLOG_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define USDWRITE_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define PCA9685_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define CMD_HIGH_LEVEL_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define MULTIRANGER_TASK_STACKSIZE (1* configBASE_STACK_SIZE)
#define ACTIVEMARKER_TASK_STACKSIZE configBASE_STACK_SIZE
#define AI_DECK_TASK_STACKSIZE configBASE_STACK_SIZE
#define UART2_TASK_STACKSIZE configBASE_STACK_SIZE
任务优先级配置
~~~~~~~~~~~~~~
系统 TASK 优先级可以在 ``components/config/include/config.h``
中进行配置。由于 ``ESP32`` 具有双核优势，相比 ``ESP32-S2``
计算资源更加富余，可将高耗时的 ``KALMAN_TASK`` 优先级调高。在使用
``ESP32-S2`` 时，需要将高耗时的 ``KALMAN_TASK``
优先级调低，否则难以释放足够的 CPU 资源，将触发 task watchdog。
::
// 任务优先级，数字越大，优先级越高。
#define STABILIZER_TASK_PRI 5
#define SENSORS_TASK_PRI 4
#define ADC_TASK_PRI 3
#define FLOW_TASK_PRI 3
#define MULTIRANGER_TASK_PRI 3
#define SYSTEM_TASK_PRI 2
#define CRTP_TX_TASK_PRI 2
#define CRTP_RX_TASK_PRI 2
#define EXTRX_TASK_PRI 2
#define ZRANGER_TASK_PRI 2
#define ZRANGER2_TASK_PRI 2
#define PROXIMITY_TASK_PRI 0
#define PM_TASK_PRI 0
#define USDLOG_TASK_PRI 1
#define USDWRITE_TASK_PRI 0
#define PCA9685_TASK_PRI 2
#define CMD_HIGH_LEVEL_TASK_PRI 2
#define BQ_OSD_TASK_PRI 1
#define GTGPS_DECK_TASK_PRI 1
#define LIGHTHOUSE_TASK_PRI 3
#define LPS_DECK_TASK_PRI 5
#define OA_DECK_TASK_PRI 3
#define UART1_TEST_TASK_PRI 1
#define UART2_TEST_TASK_PRI 1
//if task watchdog triggered, KALMAN_TASK_PRI should set lower or set lower flow frequency
#ifdef TARGET_MCU_ESP32
#define KALMAN_TASK_PRI 2
#define LOG_TASK_PRI 1
#define MEM_TASK_PRI 1
#define PARAM_TASK_PRI 1
#else
#define KALMAN_TASK_PRI 1
#define LOG_TASK_PRI 2
#define MEM_TASK_PRI 2
#define PARAM_TASK_PRI 2
#endif
#define SYSLINK_TASK_PRI 3
#define USBLINK_TASK_PRI 3
#define ACTIVE_MARKER_TASK_PRI 3
#define AI_DECK_TASK_PRI 3
#define UART2_TASK_PRI 3
#define WIFILINK_TASK_PRI 3
#define UDP_TX_TASK_PRI 3
#define UDP_RX_TASK_PRI 3
#define UDP_RX2_TASK_PRI 3
关键任务介绍
------------
除了系统默认开启的 TASK（如 Wi-Fi TASK），优先级最高的 TASK 是
``STABILIZER_TASK``，凸显了这个任务的重要性。``STABILIZER_TASK``
控制了从传感器数据读取，到姿态计算，到目标接收，到最终输出电机功率的整个过程，驱动各个阶段的算法运行。
.. figure:: ../../_static/General-framework-of-the-stabilization-structure-of-the-crazyflie-with-setpoint-handling.png
:align: center
:alt: stabilizerTask process
:figclass: align-center
stabilizerTask 流程
.. figure:: ../../_static/stabilizerTask.png
:align: center
:alt: stabilizerTask
:figclass: align-center
stabilizerTask
传感器驱动
----------
传感器驱动代码，可以在 ``components\drivers`` 中查阅。``drivers``
使用了与
`esp-iot-solution <https://github.com/espressif/esp-iot-solution/>`__
类似的文件结构，将驱动程序按照所属总线进行分类，包括
``i2c_devices``、``spi_devices``、``general``
等。具体可参考：`驱动程序 <./drivers>`__。
.. figure:: ../../_static/drivers_flie_struture.png
:align: center
:alt: drivers_flie_struture
:figclass: align-center
驱动文件结构
传感器硬件抽象
--------------
``components\core\crazyflie\hal\src\sensors.c``
文件对传感器进行了硬件抽象，开发者可以自由组合传感器，通过实现硬件抽象层定义的传感器接口，与上层应用进行对接。
::
typedef struct {
SensorImplementation_t implements;
void (*init)(void);
bool (*test)(void);
bool (*areCalibrated)(void);
bool (*manufacturingTest)(void);
void (*acquire)(sensorData_t *sensors, const uint32_t tick);
void (*waitDataReady)(void);
bool (*readGyro)(Axis3f *gyro);
bool (*readAcc)(Axis3f *acc);
bool (*readMag)(Axis3f *mag);
bool (*readBaro)(baro_t *baro);
void (*setAccMode)(accModes accMode);
void (*dataAvailableCallback)(void);
} sensorsImplementation_t;
ESP-Drone 实现的传感器抽象接口在
``components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c``
中，通过以下赋值过程与上层应用对接：
::
#ifdef SENSOR_INCLUDED_MPU6050_HMC5883L_MS5611
{
.implements = SensorImplementation_mpu6050_HMC5883L_MS5611,
.init = sensorsMpu6050Hmc5883lMs5611Init,
.test = sensorsMpu6050Hmc5883lMs5611Test,
.areCalibrated = sensorsMpu6050Hmc5883lMs5611AreCalibrated,
.manufacturingTest = sensorsMpu6050Hmc5883lMs5611ManufacturingTest,
.acquire = sensorsMpu6050Hmc5883lMs5611Acquire,
.waitDataReady = sensorsMpu6050Hmc5883lMs5611WaitDataReady,
.readGyro = sensorsMpu6050Hmc5883lMs5611ReadGyro,
.readAcc = sensorsMpu6050Hmc5883lMs5611ReadAcc,
.readMag = sensorsMpu6050Hmc5883lMs5611ReadMag,
.readBaro = sensorsMpu6050Hmc5883lMs5611ReadBaro,
.setAccMode = sensorsMpu6050Hmc5883lMs5611SetAccMode,
.dataAvailableCallback = nullFunction,
}
#endif
传感器校准过程
--------------
陀螺仪校准过程
~~~~~~~~~~~~~~
由于陀螺仪存在较大的温漂，因此每次使用前需要对陀螺仪进行校准，计算当前环境下的陀螺仪基准值。ESP-Drone
延续 Crazyflie 2.0
陀螺仪校准方案，在初次上电时，计算陀螺仪三个轴的方差与平均值。
陀螺仪具体校准过程如下：
1. 使用一个最大长度为 1024 的环形缓冲区，存储最新的 1024
组陀螺仪测量值。
2. 通过计算陀螺仪输出值方差，确认飞行器已经放置平稳并且陀螺仪工作正常。
3. 确认第 2 步正常后，计算静止时 1024
组陀螺仪输出值的平均值，作为陀螺仪的校准值。
**陀螺仪基准值计算源代码：**
::
/**
* Adds a new value to the variance buffer and if it is full
* replaces the oldest one. Thus a circular buffer.
*/
static void sensorsAddBiasValue(BiasObj* bias, int16_t x, int16_t y, int16_t z)
{
bias->bufHead->x = x;
bias->bufHead->y = y;
bias->bufHead->z = z;
bias->bufHead++;
if (bias->bufHead >= &bias->buffer[SENSORS_NBR_OF_BIAS_SAMPLES])
{
bias->bufHead = bias->buffer;
bias->isBufferFilled = true;
}
}
/**
* Checks if the variances is below the predefined thresholds.
* The bias value should have been added before calling this.
* @param bias The bias object
*/
static bool sensorsFindBiasValue(BiasObj* bias)
{
static int32_t varianceSampleTime;
bool foundBias = false;
if (bias->isBufferFilled)
{
sensorsCalculateVarianceAndMean(bias, &bias->variance, &bias->mean);
if (bias->variance.x < GYRO_VARIANCE_THRESHOLD_X &&
bias->variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
bias->variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
(varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
{
varianceSampleTime = xTaskGetTickCount();
bias->bias.x = bias->mean.x;
bias->bias.y = bias->mean.y;
bias->bias.z = bias->mean.z;
foundBias = true;
bias->isBiasValueFound = true;
}
}
return foundBias;
}
**修正陀螺仪输出值：**
::
sensorData.gyro.x = (gyroRaw.x - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;
sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
applyAxis3fLpf((lpf2pData *)(&gyroLpf), &sensorData.gyro); //低通滤波器，去除高频干扰
加速度计校准过程
~~~~~~~~~~~~~~~~
重力加速度校准
^^^^^^^^^^^^^^
在地球不同的纬度和海拔下，重力加速度 g
值一般不同，因此需要使用加速度计对 g 进行实际测量。可参考 Crazyflie 2.0
加速度计校准方案，g 值的校准过程如下：
1. 陀螺仪校准完成后，立刻进行加速度计校准。
2. 使用 Buffer 保存 200 组加速度计测量值。
3. 通过合成重力加速度在三个轴的分量，计算重力加速度在静止状态下的值。
参考：`不同地球纬度和海拔下的重力加速度值
g <https://baike.baidu.com/item/%E9%87%8D%E5%8A%9B%E5%8A%A0%E9%80%9F%E5%BA%A6/23553>`__。
**计算静止状态下重力加速度值：**
::
/**
* Calculates accelerometer scale out of SENSORS_ACC_SCALE_SAMPLES samples. Should be called when
* platform is stable.
*/
static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
{
static bool accBiasFound = false;
static uint32_t accScaleSumCount = 0;
if (!accBiasFound)
{
accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
accScaleSumCount++;
if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
{
accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
accBiasFound = true;
}
}
return accBiasFound;
}
**通过实际重力加速度值，修正加速度计测量值：**
::
accScaled.x = (accelRaw.x) * SENSORS_G_PER_LSB_CFG / accScale;
accScaled.y = (accelRaw.y) * SENSORS_G_PER_LSB_CFG / accScale;
accScaled.z = (accelRaw.z) * SENSORS_G_PER_LSB_CFG / accScale;
机身水平校准
^^^^^^^^^^^^
理想状态下，加速度传感器在小飞机上完全水平地进行安装，进而可以使用 0
位置作为小飞机的水平面。但由于加速度计在安装时不可避免的存在一定的倾角，导致飞控系统错误估计水平位置，导致小飞机向某个方向偏飞。因此需要设置一定的校准策略来平衡这种误差。
1. 将小飞机放置在一个水平面上，计算小飞机 ``cosRoll``、``sinRoll``、``cosPitch``、``sinPitch``。理想状态下 ``cosRoll``、``cosPitch`` 为 1，``sinPitch``、``sinRoll`` 为 0。如果不是水平安装 ``sinPitch``、``sinRoll`` 不为 0，``cosRoll`` ``cosPitch`` 不为 1。
2. 将步骤 1 的 ``cosRoll``、``sinRoll``、``cosPitch``、``sinPitch`` 或对应的 ``Roll``、``Pitch`` 角度值保存到飞机，用于校准。
**利用校准值，对加速度计测量值进行修正：**
::
/**
* Compensate for a miss-aligned accelerometer. It uses the trim
* data gathered from the UI and written in the config-block to
* rotate the accelerometer to be aligned with gravity.
*/
static void sensorsAccAlignToGravity(Axis3f *in, Axis3f *out)
{
//TODO: need cosPitch calculate firstly
Axis3f rx;
Axis3f ry;
// Rotate around x-axis
rx.x = in->x;
rx.y = in->y * cosRoll - in->z * sinRoll;
rx.z = in->y * sinRoll + in->z * cosRoll;
// Rotate around y-axis
ry.x = rx.x * cosPitch - rx.z * sinPitch;
ry.y = rx.y;
ry.z = -rx.x * sinPitch + rx.z * cosPitch;
out->x = ry.x;
out->y = ry.y;
out->z = ry.z;
}
以上过程，可通过力的分解和勾股定理推导。
姿态计算
--------
支持的姿态计算算法
~~~~~~~~~~~~~~~~~~
- 互补滤波
- 卡尔曼滤波
ESP-Drone 姿态计算代码来自 ``Crazyflie``。ESP-Drone
固件已经对互补滤波和卡尔曼滤波进行了实际测试，可以有效地计算飞行姿态，包括各个自由度的角度、角速度、和空间位置，为控制系统提供了可靠的状态输入。需要注意的是，在定点模式下，必须切换到卡尔曼滤波算法，才能保证工作正常。
Crazyflie 状态估计见 `State estimation: To be or not to
be! <https://www.bitcraze.io/2020/01/state-estimation-to-be-or-not-to-be/>`__
互补滤波
~~~~~~~~
.. figure:: ../../_static/Schematic-overview-of-inputs-and-outputs-of-the-Complementary-filter.png
:align: center
:alt: Extended-Kalman-Filter
:figclass: align-center
互补滤波
互补滤波中文说明可参考 `飞控与姿态互补滤波器 <https://zhuanlan.zhihu.com/p/34323865>`__。
卡尔曼滤波
~~~~~~~~~~
.. figure:: ../../_static/Schematic-overview-of-inputs-and-outputs-of-the-Extended-Kalman-Filter.png
:align: center
:alt: Extended-Kalman-Filter
:figclass: align-center
卡尔曼滤波
卡尔曼滤波中文说明可参考 `图说卡尔曼滤波，一份通俗易懂的教程 <https://zhuanlan.zhihu.com/p/39912633>`__。
控制算法
--------
已支持的控制器
~~~~~~~~~~~~~~
ESP-Drone 控制系统代码来自 ``Crazyflie``，也继承了该工程的所有控制算法。需要注意的是，ESP-Drone 仅对 PID 控制器进行了参数整定和测试。换用其它控制器时，请在确保安全的情况下，自行进行参数整定。
.. figure:: ../../_static/possible_controller_pathways.png
:align: center
:alt: possible_controller_pathways
:figclass: align-center
possible_controller_pathways
详情请参考：`Out of Control <https://www.bitcraze.io/2020/02/out-of-control/>`__。
在代码中，可通过修改 ``controllerInit(ControllerType controller)`` 的传入参数，切换控制器。
也可通过实现以下控制器接口，添加自定义的控制器：
::
static ControllerFcns controllerFunctions[] = {
{.init = 0, .test = 0, .update = 0, .name = "None"}, // Any
{.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"},
{.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"},
{.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"},
};
PID 控制器
~~~~~~~~~~
**控制原理**
PID 控制器（比例-积分-微分控制器），由比例单元 (Proportional)、积分单元
(Integral) 和微分单元 (Derivative)
组成，分别对应当前误差、过去累计误差及未来误差，最终基于误差和误差的变化率对系统进行控制。PID
控制器由于具有负反馈修正作用，一般被认为是最适用的控制器。通过调整 PID
控制器的三类参数，可以调整系统对误差的反应快慢、控制器过冲的程度及系统震荡的程度，使系统达到最优状态。
在飞行器系统中，由于存在 ``pitch``、 ``roll``、``yaw``
三个自由度，因此需要设计如下图所示的具有控制闭环的 PID 控制器。
.. figure:: https://img-blog.csdnimg.cn/20190929142813169.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIwNTE1NDYx,size_16,color_FFFFFF,t_70
:align: center
:alt: Crazyflie控制系统
:figclass: align-center
Crazyflie 控制系统
其中每一个自由度都包括一个串级 PID 控制器：Rate 控制和 Attitude
控制，前者以角速度作为输入量，控制角度修正的速度；后者以拟合后的角度为输入量，控制飞机到达目标角度，两个控制器以不同的频率配合工作。当然，也可以选择只使用单级的
PID 控制，默认情况下 pitch 和 roll 自由度使用 Attitude 控制，yaw 使用
Rate 控制。
::
可以在 crtp_commander_rpyt.c 中调整如下参数选择
static RPYType stabilizationModeRoll = ANGLE; // Current stabilization type of roll (rate or angle)
static RPYType stabilizationModePitch = ANGLE; // Current stabilization type of pitch (rate or angle)
static RPYType stabilizationModeYaw = RATE; // Current stabilization type of yaw (rate or angle)
**实现代码**
::
void controllerPid(control_t *control, setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const uint32_t tick)
{
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { //该宏定义用于控制 PID 的计算频率，时间基准来自 MPU6050 触发的中断
// Rate-controled YAW is moving YAW angle setpoint
if (setpoint->mode.yaw == modeVelocity) { //rate 模式,对 yaw 做修正
attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT;
while (attitudeDesired.yaw > 180.0f)
attitudeDesired.yaw -= 360.0f;
while (attitudeDesired.yaw < -180.0f)
attitudeDesired.yaw += 360.0f;
} else { //attitude 模式
attitudeDesired.yaw = setpoint->attitude.yaw;
}
}
if (RATE_DO_EXECUTE(POSITION_RATE, tick)) { //位置控制
positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
}
if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
// Switch between manual and automatic position control
if (setpoint->mode.z == modeDisable) {
actuatorThrust = setpoint->thrust;
}
if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
attitudeDesired.roll = setpoint->attitude.roll;
attitudeDesired.pitch = setpoint->attitude.pitch;
}
attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
&rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
// For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
// value. Also reset the PID to avoid error buildup, which can lead to unstable
// behavior if level mode is engaged later
if (setpoint->mode.roll == modeVelocity) {
rateDesired.roll = setpoint->attitudeRate.roll;
attitudeControllerResetRollAttitudePID();
}
if (setpoint->mode.pitch == modeVelocity) {
rateDesired.pitch = setpoint->attitudeRate.pitch;
attitudeControllerResetPitchAttitudePID();
}
// TODO: Investigate possibility to subtract gyro drift.
attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
attitudeControllerGetActuatorOutput(&control->roll,
&control->pitch,
&control->yaw);
control->yaw = -control->yaw;
}
if (tiltCompensationEnabled)
{
control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
}
else
{
control->thrust = actuatorThrust;
}
if (control->thrust == 0)
{
control->thrust = 0;
control->roll = 0;
control->pitch = 0;
control->yaw = 0;
attitudeControllerResetAllPID();
positionControllerResetAllPID();
// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = state->attitude.yaw;
}
}
Mellinger 控制器
~~~~~~~~~~~~~~~~
Mellinger 控制器是一种 **多合一**
控制器，基于目标位置和目标位置速度矢量，直接计算出需要分配给所有电动机的所需推力。
详情可参考论文：`Minimum snap trajectory generation and control for quadrotors <https://ieeexplore.ieee.org/abstract/document/5980409>`__。
INDI 控制器
~~~~~~~~~~~
INDI 控制器是立即处理角速率以确定信任度的控制器，与传统的 PID
控制器相结合，对于角度处理相比串级 PID 控制器组合的速度要快。
详情可参考论文：`Adaptive Incremental Nonlinear Dynamic Inversion for
Attitude Control of Micro Air
Vehicles <https://arc.aiaa.org/doi/pdf/10.2514/1.G001490>`__。
PID 参数整定
------------
``Rate PID`` 整定
~~~~~~~~~~~~~~~~~~
1. 先调整 ``Rate`` 模式，将 ``rollType``、``pitchType`` 和 ``yawType`` 都调整为 ``RATE``；
2. 将 ``ATTITUDE`` 模式对应的 ``roll``、``pitch`` 和 ``yaw`` 的 ``KP``、``KI`` 和 ``KD`` 调整为 ``0.0``，仅保留 ``Rate`` 相关的参数；
3. 将 ``RATE`` 模式对应的 ``roll``、``pitch`` 和 ``yaw`` 的 ``KI`` 和 ``KD`` 调整为 ``0.0``，先调整比例控制 ``KP``；
4. 烧写代码，使用 cfclient 的 param 功能开始在线进行 ``KP`` 的调整；
5. 注意，使用 cfclient 修改后的参数，掉电不保存；
6. 在 PID 调整期间会出现震荡（超调）的情况，请注意安全；
7. 先固定住飞行器，让其只能进行 ``pitch`` 轴的翻转。逐渐增加 ``pitch`` 对应的 ``KP``，直到飞机出现前后的震荡；
8. 当出现严重的震荡时，可以稍微降低 ``KP``，以恰好达到震荡的临界点为基础，降低 5-10 个百分点即可确定 ``KP`` 参数；
9. 使用同样的方法调整 ``roll`` 和 ``yaw``；
10. 调整 ``KI``，该参数用于消除稳态误差。如果不引入该参数，只有比例调整的话，飞行器受到重力等干扰会在 0 位置上下摆动。设置 ``KI`` 的初始值为 ``KP`` 的 50%；
11. 当 ``KI`` 增大到一定程度，也会导致飞机不稳定晃动。但 ``KI`` 造成的晃动频率相比 ``KP`` 带来的震动，频率更小。以恰好造成震动的临界 ``KI`` 为基础，减小 5-10 个百分点，确定最终的 ``KI`` 值；
12. 使用同样的方法调整 ``roll`` 和 ``yaw``；
13. 一般情况下 ``KI`` 的取值为 ``KP`` 取值的 80% 以上。
``Attitude PID`` 整定
~~~~~~~~~~~~~~~~~~~~~~
1. 确保 ``Rate PID`` 调整已经完成；
2. 将 ``rollType``、``pitchType`` 和 ``yawType`` 都调整为 ``ANGLE``，即飞机已进入 attitude mode；
3. 改变 ``roll`` 和 ``pitch`` 的 ``KI`` 和 ``KD`` 为 ``0.0``，将 ``Yaw`` 的 ``KP``、``KI``、``KD`` 都设置为 ``0.0``；
4. 烧写代码，使用 cfclient 的 param 功能开始在线进行 ``KP`` 的调整；
5. 将 ``roll`` 和 ``pitch`` 的 ``KP`` 设置为 ``3.5``，查找任何存在的不稳定性，例如振荡。持续增加 KP，直到达到极限；
6. 如果发现 ``KP`` 导致不稳定，如果此时已经高于 ``4``，需要将 ``RATE`` 模式的 ``KP`` 和 ``KI`` 稍微降低 5 ~ 10 点。实现调整姿势模式时更加自由；
7. 要调整 KI，请再次缓慢增加 KI。不稳定性的状态是产生低频振荡。