107 lines
5.2 KiB
Markdown
107 lines
5.2 KiB
Markdown
# DJI Gimbal FOC
|
|
|
|
The aim of this project is to be able to use the 3-axis DJI gimbal with a custom open source controller like [SimpleFOC](https://docs.simplefoc.com/). This high quality gimbal is very tiny and easy to find as a replacement part which makes it very suitable for DIY projects.
|
|
|
|
<img src="docs/overview.jpg" height=250>
|
|
<img src="docs/working.gif" height=250>
|
|
|
|
## Pinout identification
|
|
The Gimbal is composed of a flex PCB with a main connector and 3 smaller for each motor. The main end connector is a 40-pin mezzanine board to board connectors. In order to work easily I have designed a breakout board which open to a 2.54" header.
|
|
|
|
Here is the strategy I followed to find the pinout:
|
|
1. Find all equipotential pins with a multimeter set to continuity tests, and test all the combinations
|
|
|
|
2. Group remaining pins by motor With the multimeter find all the pins connected to the motor connector. (Reapeat 3 times for the other connectors)
|
|
|
|
<img src="Kicad/Breakout_DJI_Gimbal.png" height=400> <img src="docs/setup.jpg" height=400>
|
|
|
|
### Open-loop control
|
|
|
|
Each motor has its own drivers a MP6536. Which makes it easy as no additional hardware is necessary to drive the motors.
|
|
There are 4 pins from the MP6536:
|
|
1. PWM1
|
|
2. PWM2
|
|
3. PWM3
|
|
4. Fault : Output. When low, indicates overtemperature, over-current, or under-voltage.
|
|
|
|
Connected directly to a MCU (here a STM32 Nucleo F401RE) and with the Simple FOC Library, open-loop control works quite well. However due to open-loop control, it cannot know when a "step" is missed so misalignment can occur. Also, the motor tends to become quite hot due to the continuous current sent to the coils.
|
|
|
|
## Position estimation with the integrated linear hall sensors
|
|
|
|
### 1. Setup
|
|
Each motor is composed of two ratiometric linear hall sensors. (Texas Instrument DRV5053 Analog-Bipolar Hall Effect Sensor) They are placed at around 120º from each other (eyes measured) and measure the magnetic field of the rotor.
|
|
|
|
<img src="docs/Hallmotor.jpg" height=300>
|
|
|
|
Ratiometric means that the output signal is proportional to the voltage supply to the sensor. In this setup, with 5V supply, the output measured is between 520mV and 1.5V, so a 1V amplitude.
|
|
|
|
### 2. Measures
|
|
|
|
These oscilloscope traces are the sensor output when rotating the rotor forth and back. (a bit less than 180º on the 3rd motor)
|
|
The channel 0 (Yellow) is the Hall 1 and the Channel 1 (Green) is the Hall 2
|
|
|
|
<img src="docs/courbes.png" height=250> <img src="docs/cosSinEncoderDiagram.png" height=250>
|
|
|
|
We can see that in the first movement (positive rotation), the green is out of phase of π/2.`
|
|
|
|
### 3. Encoding the position
|
|
1. Get the absolute angle within a period
|
|
|
|
Since the 2 signals correspond to a cos and sin signals, it is possible to compute the angle inside the period using arctan2 function. However, we have more than one period, it is so necessary to increment a position.
|
|
$$\theta= atan2(a,b)$$
|
|
|
|
2. Incremental position
|
|
|
|
To increment the position, it is necessary to start from 0 at a known postion. For that the motor is moved in open loop to one end and the position is set to 0.
|
|
Then we need to sum all the delta of movement at each measure sample.
|
|
$$\phi_t=\phi_{t-1} + (\theta_t - \theta_{t-1})mod(-\pi;\pi)$$
|
|
|
|
## Coding the solution
|
|
1. Get the angle in the perdiod
|
|
|
|
In order to compute the angle from the cos and sin with atan, it is necessary to remap the values of the analog readings from -1 to 1.
|
|
Beforehand, the maximum and minimum peak of the signals need to be found. It can be done by swiping the motor on startup in open-loop mode.
|
|
Then the arctan function can be applied. It is preferable to use arctan2 as it will give an angle within the 4 quadrants (-π,π). Whereas arctan give an angle between (-π/2,π/2). [Wikipedia](https://en.wikipedia.org/wiki/Atan2)
|
|
|
|
```C++
|
|
|
|
float LinearHallSensor::Callback() // Return the estimated position of the sensor
|
|
{
|
|
A = norm(analogRead(CH1),minCh1, maxCh1); //read analog values and normalise between [-1;1]
|
|
B = norm(analogRead(CH2),minCh2, maxCh2);
|
|
|
|
theta = atan2(A,B); // Compute the absolute angle in the period
|
|
|
|
phi = phi + dist_angle(theta, theta_prev); // increment the difference
|
|
|
|
theta_prev = theta; // save fot nex time
|
|
|
|
return phi;
|
|
}
|
|
|
|
float norm(float x, float in_min, float in_max) //return the input value normalised between [-1;1]
|
|
{
|
|
return (float)(x + 1.0) * (2.0) / (float)(in_max - in_min) -1.0;
|
|
}
|
|
|
|
float dist_angle(float newAngle, float prevAngle) // return the difference modulo [-pi;pi]
|
|
{
|
|
float diff = newAngle - prevAngle;
|
|
while (diff < (-M_PI))
|
|
diff += 2 * M_PI;
|
|
while (diff > M_PI)
|
|
diff -= 2 * M_PI;
|
|
return diff;
|
|
}
|
|
|
|
```
|
|
|
|
## Tuning the PIDs
|
|
To achive position control it is necessary to have first, a velocity controller well tuned, as they are in cascade. (SimpleFOC implementation and diagram)
|
|
|
|
|
|
However, the motors of the gimbal have hard stop and can only rotate of around a half turn. It was so necessary to remove these mecanical stops. I drilled with a 1.6mm drill the two little holes to remove it. Then the motor was able to rotate freely and PID can be tuned.
|
|
<img src="docs/drilling.jpg" height=250> <img src="docs/freeturn.gif" height=250>
|
|
|
|
|