I wanted to replace the PID algorithm in mc_rate_control with a custom MPC algorithm. To achieve this, I added a custom function: Vector3f RateControl::MPC(const Vector3f &rate, const Vector3f &rate_sp) in src/lib/rate_control/rate_control.cpp, and replaced the line in src/modules/mc_rate_control/MulticopterRateControl.cpp from att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); to att_control = _rate_control.MPC(rates, _rates_setpoint);
To test the performance of the MPC algorithm, I conducted a software-in-the-loop simulation. I switched to ACRO mode, unlocked the system, and then gradually increased the throttle. Aside from that, no other actions were taken.
When the throttle reached a certain level, the drone began to oscillate on the ground. No matter how much I increased the throttle, the drone continued to oscillate on the ground and could not take off. By analyzing the logs, I discovered that when I increased the throttle, the speeds of two motors increased, while the other two motors, due to their control_allocator_status being -2 (indicating that these two motors had reached their minimum limit), could not increase their speed.
Why did two of the motors reach their minimum limit?
At the same time, I found that the two slower motors were not arranged diagonally.
（I guess that when the two faster motors reach a speed sufficient for the drone to lift off, the two slower motors begin to increase their speed, causing the drone to fail to take off and oscillate on the ground.）
Thanks for your reply. I have thoroughly reviewed the contents of the repository and found that the MPC algorithm is designed to run on the onboard computer, with the output subsequently sent to the Pixhawk.
The issue I am facing is that I only have the Pixhawk 6x available, without an onboard computer, and I aim to run the MPC algorithm directly on the Pixhawk.
Currently, I have modified the PID algorithm in the angular velocity loop for attitude control, while leaving the angle loop unchanged. Is this approach feasible? Are there any additional recommendations or best practices?
Did you try taking off with the original control loops? I am asking to be sure that your custom changes are the only difference from a functional machine.
After that, you should make sure your algorithm recognizes the oscillations and takes them into account for its output.
I’m guessing your function’s output needs to be scaled down since it looks like it outputs either 0 or 100 once some movement is identified.
You’re also not taking into account angular_accel which is the derivative in PX4’s original PID loop.
Thanks for your reply. I have test the original control loops. It’s OK. The only difference between original control loops and my customized algorithms is the control function in mc_rate_control.
Do you mean that I need to ensure my algorithm can eliminate this oscillation?
My algorithm’s output amplitude might indeed need some adjustment; what you said makes sense.
In my MPC derivation process, the angular acceleration information was not used.
Let us know if you made it work.
By the way - you mentioned 2 motors going to minimum limit, but we can see there are points where all motors going to 0 or to 100.
int8 ACTUATOR_SATURATION_LOWER_DYN = -1 # The actuator is saturated (with a value >= the desired value) because it cannot decrease its value faster
int8 ACTUATOR_SATURATION_LOWER = -2 # The actuator is saturated (with a value >= the desired value) because it has reached its minimum value
In the area indicated by the red box, as the throttle increases, the upper arrow shows the output of two motors, and at this point, the state of the two motors indicated by the lower arrow has reached the minimum limit.
