I'm working on a quadcopter simulator based on Unreal Engine 4. The motion of the quadcopter is physics-based and will make use of an actual RC controller eventually (for now I'm using a XBox Controller). It's working quite well so far but I have a problem when attempting to control the yaw and I'd like to ask for your help.
This is where I am now in my simulation (just in case I made a mistake):
- I look the position of the sticks, and a PWM signal (between 1000 and 2000 us) is sent to the ESCs based on the PIDs
- there is 2 modes: acrobatic and stabilised. In acrobatic mode, the sticks control the rate of rotation and 1 PID per axis is used. In stabilised mode, one PID is added on each axis, the first one takes the requested angle as input, produced a requested rate based on the error, which feeds the second PID (the same one used in acrobatic mode) to produce a correction on the PWM signal for each axis (roll, pitch and yaw)
- the mixing occurs on the motors (X flight mode only for now) the following way:
Front Left = thrust + roll_pid + pitch_pid + yaw_pid
Front Right = thrust - roll_pid + pitch_pid - yaw_pid
Rear Left = thrust + roll_pid - pitch_pid - yaw_pid
Rear Right = thrust - roll_pid - pitch_pid + yaw_pid
where "thrust" is the thrust commanded by the position of the thrust stick and the *_pid the corrections computed by the last PIDs
- the PWM values for each motor is converted into a propeller speed (simple mapping from the range 1000-2000 to 0 - max_speed where max_speed = Kv_rating * max_voltage)
- the propeller speed is converted into a lift force for each propeller based on the following formula:
Lift force = Ct * D^4 * rho * w^2
where T is the thrust, Ct a constant (obtained from the UIUC propeller database), rho is the air density (1.225), D is the propeller diameter (in meters) and w the propeller speed in revolutions per second.
- the torque produced by a propeller is computed from a similar formula:
Torque = Cp * D^5 * rho * w^2
where Cp is a constant (obtained from the UIUC propeller database)
I started by roll and pitch control (without computing the torque) and it works well (I attached a screenshot just for your curiosity). The quadcopter is quite responsive in my simulator and when I release the roll/pitch stick, the quadcopter comes back to its horizontal position. So the control seems to work fine (even though I'm only using proportional corrections so far... I'm planning to add the integral part very soon). Of course I can turn right/left, forward and backward by changing the propeller speeds on my motors. And although the center of mass of my quadcopter mesh is not in the middle of my 4 propellers, I can take off by only adding thrust, my control will compensate by commanding slightly different propeller speeds for each of the propellers (it basically works as expected).
So I want to add the control of yaw now. I added the modelling of the torque forces for each propeller. The control is slightly different than roll and pitch. If the user uses the yaw stick, I feed the command directly to the yaw rate controller and update the target angle. When the pilot releases the stick, I add another PID in front and try to keep the last target angle (right before the pilot release the sitck). And the control seeks to work to some extend (it corrects the rotation as expected).
Now my problem: when I use the yaw stick, my quadcopter increases substantially the total thrust and goes up in the air very fast. And I understand why it happens but I don't understand why it does not happen in real life. All the things I have read on quadcopter controls seems to present things like I did them. If I'm not mistaken, the mixing on the motors of the PID corrections are for correcting the PWM signals. And I read that you should reduce the signal sent to a motor by the same amount you add on the opposite motor. This is why we add roll_pid on left motors and substract the same amount roll_pid on right motors. But the lift force generated is not linear, it's actually proportional to the speed squared. So if we have a PWM signal sent to each motor with a value "x", the total thrust on the quad is proportianal to:
total thrust = constant * x^2 + constant * x^2 + constant * x^2 + constant * x^2
Now if we add 10 to the previous PWM signal "x" for clockwise motors and substract the same amount to the counterclockwise motors, the new thrust for a clockwise motor is:
thrust = constant * (x+10)^2 = constant * [ x^2 + 20x + 10^2 ] = previous thrust + constant * (20x + 10^2)
And the new thrust for a counterclockwise motor is:
thrust = constant * (x-10)^2 = constant * [ x^2 - 20x + 10^2 ] = previous thrust + constant * (-20x + 10^2)
So the new total thrust = previous_total_thrust + 4 * constant * 10^2.
So the thrust basically increases, which is consistent with what I'm seeing in my simulation. Since in real life the quad don't go up like crazy when we control the yaw axis, I was wondering what I'm doing wrong. For me, if we substract the same pid correction than we add on the opposite motors (like I see everyone doing when talking about quad control so far), there's no way that the thrust is maintained.
What am I missing? Any help would be greatly appreciated. Thanks.