Jump to content
  • Advertisement

Quadcopter simulation and PIDs

Recommended Posts

I'm trying to write a simple quadcopter simulation. It doesn't need to be 100% accurately simulated and I don't intend to write an actual drone controller, it's just supposed to "look" right in the simulation, and I'm trying to get a better understanding of PIDs. As such, I don't need to worry about sensors, or an accelerometer or gyroscope, I get all of the information from the simulation directly.

I'm new to quadcopter physics and PIDs, so there are a few things I'm a bit unclear on. I'm not asking for concrete solutions, I'd just like to know if my ideas go in the right direction and/or if I'm missing anything.


Let's say the quadcopter has the world coordinates Q(0 -50 0) (y-axis being the up-axis) and I want it to fly to O(0 0 0). The velocity I need is O -Q, so I need to accelerate the drone until I reach that velocity (Correcting it every simulation tick). To do that, I need a PID, with the setpoint being the desired velocity O -Q, and the process variable being the current velocity of the drone (both on the y-axis), and 1/4th of the PID output is then applied as force to each rotor (Let's ignore the RPM of the rotors and assume I can just apply force directly). Is that correct?


The drone is at the world coordinates Q(0 0 0) and should fly to P(100 0 100). It also has the orientation (p(itch)=0, y(aw)=0 and r(oll)=0). I want the drone to fly to point Pwithout changing its yaw-orientation. How would I calculate the target pitch- and roll-angles I need? I can work with euler angles and quaternions (not sure if I need to worry about gimbal lock in this case), I just need to know what formulas I need. Of course I also want to make sure the drone doesn't overturn itself.


I want to change the drone's orientation from (p=0, y=0, r=0) to (p=20, y=0 r=0) (With positive pitch pointing downwards). To do so, I'd have to increase the force that is being applied by the front rotors, and/or decrease the force of the back rotors. I can use a PID to calculate the angular velocity I need for the drone (similar as in 1) ), but how do I get from there to the required forces on each rotor?


Am I correct in assuming that I need 6 PIDs in total, 3 for the angular axes and 3 for the linear axes?

Share this post

Link to post
Share on other sites
1 hour ago, Silverlan said:


The velocity I need is O -Q

That's the velocity needed to travel from Q to O in one time unit.  Which...is probably not what you want? But yes, if you want to move at a certain velocity then your setpoint would be the target velocity and your process variable would be the current velocity.  But then you have to figure out how to set the target velocity over the whole path, and it's not so great for maintaining position.  I would more likely have the setpoint be a target location (and orientation) and the process variable be the current location.  Then you can plan a path and move the target along it at a comfortable cruising speed and the PIDs should make it follow fairly nicely.

Also, the "apply 1/4 force to each rotor" isn't exactly correct.  Maybe you were just glossing over the details?  To accelerate, you need to tip the quadcopter in the direction you want to accelerate and then apply enough force to the rotors to maintain height.  So the horizontal acceleration you get is based on the orientation as much as the rotor lift.


2) I would think you would use the cross-product of the yaw (up) axis and the direction that you want to go to get an axis of rotation.  Then convert axis-angle (I want to tilt this much on this axis) and convert to yaw/pitch/roll so you can apply it to the rotors easily (left/right pairs and forward/back pairs).  Here I would definitely feed the orientations to the PID rather than the angular velocities. You can probably get away without worrying about gimbal lock for a while since you'll only be making small orientation changes. But if something bad happens (you crash into something, or whatever) then you might run into trouble.


3) I would do this empirically: that's kind of what the PID is for.  Feed in the target orientation and the current orientation, and then tune the PID until the output values give you nice behavior.  IIRC Wikipedia has a procedure for manual tuning that works pretty well.


4) Yes, the linear axes are independent, so one PID for each.  There might be some better way to do the angular one?  But one per axis should work well enough to get you up and running...

Share this post

Link to post
Share on other sites

Controlling the position of the drone is a very high level task, so you're coming at this from a top-down direction. I would recommend going bottom-up instead.

In order of simplicity, I'd make a PID controller for:

  1. Rate / acro mode: This is what "drone racers" fly with. Stabilization is based on a gyroscope (or angular velocity from your physics system). The PID controller trying to reduce the error between a requested angular velocity (from the pilot's sticks) and the measured angular velocity. When you let go of the sticks, the drone should remain at the current orientation (i.e. it should make it appear that angular momentum doesn't exist).
  2. Angle / level mode: This is what most "toy drones" fly with. Stabilization is based on an accelerometer (or orientation from your physics system). The PID controller is trying to reduce the error between the requested orientation (from the pilot's sticks) and the measured orientation. When you let go of the sticks, the drone should return to 0º pitch/roll. Yaw is actually processed the same way as in #1.
  3. Hover -- this can be added on top of either of the above. A second layer of stabilization is based on an altimeter (or Y position from the physics system). An additional PID controller is trying to maintain the requested altitude. At middle throttle this PID controller is in full effect, or at negative/positive throttle stick positions this PID controller is attenuated to allow the drone to gain/lose altitude.
  4. Positional movement from waypoint to waypoint.

Unless you already know how to fly an acrobatic drone and are interested in it, you can probably skip #1 and just do #2 instead (they are mutually exclusive controllers anyway). #3 is a secondary PID controller that is layered over the top of #2.

For doing autonomous flying (move from position to position), you would then add another PID controller on top (#4) which controls the virtual "pilot's sticks", which are fed into the PID controllers from step #2 and step #3. This layer probably doesn't even need to be a PID controller -- you can just get the direction to the target and directly feed that value to the "sticks" without any filtering at all.

So that gives:
Layer #2 -- converts the throttle/yaw and pitch/roll stick commands into motor outputs. Throttle inputs are not stabilized, but pitch/roll are converted into a target absolute orientation, and yaw is converted into a target angular velocity.
Layer #3 -- generates throttle stick movements based on a requested altitude.
Layer #4 -- generates yaw/pitch/roll stick movements based on a requested position/speed.

You should be able to disable layer 3/4 and fly it manually. 

So for your questions --
2) don't. The waypoint (#4) layer just needs to know what direction it wants to fly in and how fast. It then pushes the pitch/roll stick on that direction with a magnitude proportional to the speed that it wants. The angle stabilization layer (#2) then converts that stick position into a desired pitch/roll angle, and its PID controller adjusts the motor speeds until the drone's actual pitch/roll converges to the desired one. The conversion from stick position to angle can be as simple as:

float StickToAngle( float stick ) { /*stick is from -1 to +1*/ return stick * 30; }//at full stick movement, the drone will rotate 30 degrees.

When the drone rotates 30º, it will start to move sideways and also begin to lose altitude. The hovering layer's PID controller (#3) will then kick in due to the change in altitude, and will increase the throttle stick in order to increase thrust an maintain altitude. Layer #2 will read the throttle/yaw stick at the same time as it reads the pitch/roll stick, and adjust the motor speeds accordingly.

3) Make a map of which axis requests affect each motor. e.g. a pitch request (nose up please) will have +1 for the front two motors and -1 for the back two motors // a roll request (right wing down) will have +1 for the left two motors and -1 for the right two motors.

Make a sum for each motor and initialize it to zero. For each angular velocity output, add it to each motor based on the map.
e.g. if the request is pitch=0.5, roll=0.3
front-left = 0.5+0.3 = 0.7
front-right = 0.5-0.3 = 0.2
back-left = -0.5+0.3 = -0.2
back-right = -0.5-0.3 = -0.7
If any of these numbers go above 1.0 (100%), then you can divide all of them by the largest value to avoid loss of control during harsh manouvres. You make sure that the motors responses are not too high or too low by tweaking the P value in your Layer#2 PID.

4) Layer #2 needs 3 PIDs, Layer #3 needs 1 PID, Layer #4 needs 4 PIDs.

Share this post

Link to post
Share on other sites

Thanks, that helps quite a bit! Still having a bit of trouble, however.

For testing, I disabled angular movement for the drone, as well as movement on the x- and z- axes (so it can only move up and down), and I've tried to implement #3 (keeping a specific altitude). However, no matter what I set the parameters of the PID to, I can't get it to change the rotor speed quickly enough, so it ends up overshooting the target massively:

(The target altitude is 0, which is more or less central between the green block and the ground floor.)

Here's the test-code I've used:

local pidAltitude = math.PIDController(100,20,5) -- Parameters: proportional, integral, derivative
function PhysDrone:Simulate(dt)
  local pos = self:GetPos() -- Current position
  local posTgt = Vector(0,0,0) -- Target position; x and z are ignored for now
  local v = pidAltitude:Calculate(pos.y,posTgt.y,dt) -- Parameters: processFeedback, setpoint, delta time
  for rotorId,entRotor in ipairs(self.m_rotors) do -- for each of the 4 rotors
    entRotor:ApplyTorque(Vector(0,v,0)) -- PID output is applied as a torque force to the rotor

    local up = entRotor:GetUp() -- Normalized vector pointing upwards from the rotor (Always the same direction as the up-vector of the drone)
    local liftForce = entRotor:GetLiftForce() -- How much lift force should be applied to the drone by this rotor (Depending on current revolutions per second)
    local posRotor = self:WorldToLocal(entRotor:GetPos()) -- Where the lift force originates from, relative to the drone
    self:ApplyForce(up *liftForce *dt,posRotor)

I've tried following the tuning instructions for PIDs on wikipedia, but haven't had any luck, the result is always almost exactly what you see in the video. There are two different forces involved, which I suppose complicates things a little, but I'm not sure what to do here.

Edited by Silverlan

Share this post

Link to post
Share on other sites

What are the two forces involved?

A PID that overshoots usually can be reigned in by tweaking the D term. You may need a smaller P term too.

Share this post

Link to post
Share on other sites
13 hours ago, alvaro said:

What are the two forces involved?

The torque force of the rotors, as well as the lift force applied to the drone, as shown in the code snippet.

14 hours ago, alvaro said:

A PID that overshoots usually can be reigned in by tweaking the D term. You may need a smaller P term too.

The P-term has to be that high, otherwise the rotors take too long to reach the required RPS.


It gets increasingly unstable with higher values though.

Share this post

Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

  • Advertisement
  • Advertisement
  • Popular Tags

  • Similar Content

    • By Woody Stevens
      I am looking for a TCP or HTTP networking library similar to Lidgren (UDP).
      This is primarily for sending game map data and potentially other large messages from Server to Client.
      I do want to keep Lidgren for my chat messages, player position, small fast updates etc. I especially love the flow of data and the library usage in general, so any libraries of a similar style would be excellent. Preferably something open source, free and reliable.
      I also must be able to swap between localhost and an ip address with ease, like Lidgren, as I run a server for singleplayer/mp/lan.
      My game maps are similar to minecraft, but it is 2d and only one Z-level, so i'm sending a jagged array of Tile object data (currently only enum TileID.Grass) down the pipe to the Client. Problem is if i'm sending a large map 1024 x 1024 tiles down the to client that's quite a lot of data, and Lidgren is relatively slow to build the writes (before the message is even sent!). It is fine when i'm using smaller maps < 512 x 512 ( xTiles * yTiles ).

      I know about chunking and will look into implementing this later, whilst taking into account the user's position in the world to only send nearby chunks.
      An example of my code that can be slow:
      private void WriteWorld(NetOutgoingMessage outgoing) { try { var world = WorldManager.Instance.CurrentWorld; outgoing.Write(world.XTiles); outgoing.Write(world.YTiles); for (int x = 0; x < world.XTiles; x++) { for (int y = 0; y < world.YTiles; y++) { // Write Tile obj data outgoing.Write((int)world.Tiles[x][y]); // <-------- Slow here when xTiles and yTiles are each > 512 ! } } } catch (Exception ex) { // log send error } }  
      I'd love to hear from you guys, especially if any of you have come across a similar challenge.
    • By NanaMarfo
      Hello Everyone!
      I am looking for a small team to do a rendering project with me. The roles I need are:
      -Character Modeller
      -Environment Designer
      -Environment Modeller(Found)
      You can use this in your portfolio and you will be credited at the end.
      If you are interested, please email me at marfo343@gmail.com. Thank you!
    • By D34DPOOL
      Edit Your Profile D34DPOOL 0 Threads 0 Updates 0 Messages Network Mod DB GameFront Sign Out Add jobEdit jobDeleteC# Programmer for a Unity FPS at Anywhere   Programmers located Anywhere.
      Posted by D34DPOOL on May 20th, 2018
      Hello, my name is Mason, and I've been working on a Quake style arena shooter about destroying boxes on and off for about a year now. I have a proof of concept with all of the basic features, but as an artist with little programming skill I've reached the end of my abilities as a programmer haha. I need someone to help fix bugs, optomize code, and to implent new features into the game. As a programmer you will have creative freedom to suggest new features and modes to add into the game if you choose to, I'm usually very open to suggestions :).
      What is required:
      Skill using C#
      Experience with Unity
      Experience using UNET (since it is a multiplayer game), or the effort and ability to learn it
      Since the game currently has no funding, we can split whatever revenue the game makes in the future. However if you would perfer I can create 2D and/or 3D assets for whatever you need in return for your time and work.
      It's a very open and chill enviornment, where you'll have relative creative freedom. I hope you are interested in joining the team, and have a good day!
      To apply email me at mangemason@yahoo.com
    • By Andrew Parkes
      I am a talented 2D/3D artist with 3 years animation working experience and a Degree in Illustration and Animation. I have won a world-wide art competition hosted by SFX magazine and am looking to develop a survival game. I have some knowledge of C sharp and have notes for a survival based game with flexible storyline and PVP. Looking for developers to team up with. I can create models, animations and artwork and I have beginner knowledge of C sharp with Unity. The idea is Inventory menu based gameplay and is inspired by games like DAYZ.
      Here is some early sci-fi concept art to give you an idea of the work level. Hope to work with like minded people and create something special. email me andrewparkesanim@gmail.com.
      Developers who share the same passion please contact me, or if you have a similar project and want me to join your team email me. 
      Many thanks, Andrew.

    • By thecheeselover
      I made this post on Reddit. I need ideas and information on how to create the ground mesh for my specifications.
    • By JoAndRoPo
      While going through a Game Design Document Template, I came across this heading - Core Game Loop & Core Mechanics Loop. 
      What's the difference? Can you provide some examples of an existing game?  Suppose if I am including these topics in a Game Design Document, how should I explain it so that my team can understand? 
    • By Canadian Map Makers
      GOVERNOR is a modernized version of the highly popular series of “Caesar” games. Our small team has already developed maps, written specifications, acquired music and performed the historical research needed to create a good base for the programming part of the project.

      Our ultimate goal is to create a world class multi-level strategic city building game, but to start with we would like to create some of the simpler modules to demonstrate proof of concept and graphical elegance.


      We would like programmers and graphical artists to come onboard to (initially) create:

      A module where Province wide infrastructure can be built on an interactive 3D map of one of the ancient Roman Provinces.
      A module where city infrastructure can be built on a real 3D interactive landscape.
      For both parts, geographically and historically accurate base maps will be prepared by our team cartographer. Graphics development will be using Blender. The game engine will be Unity.


      More information, and examples of the work carried out so far can be found at http://playgovernor.com/ (most of the interesting content is under the Encyclopedia tab).


      This project represents a good opportunity for upcoming programmers and 3D modeling artists to develop something for their portfolios in a relatively short time span, working closely with one of Canada’s leading cartographers. There is also the possibility of being involved in this project to the point of a finished game and commercial success! Above all, this is a fun project to work on.


      Best regards,

      Steve Chapman (Canadian Map Makers)

    • By RobMaddison
      I’ve been working on a game engine for years and I’ve recently come back to it after a couple of years break.  Because my engine uses DirectX9.0c I thought maybe it would be a good idea to upgrade it to DX11. I then installed Windows 10 and starting tinkering around with the engine trying to refamiliarise myself with all the code.
      It all seems to work ok in the new OS but there’s something I’ve noticed that has caused a massive slowdown in frame rate. My engine has a relatively sophisticated terrain system which includes the ability to paint roads onto it, ala CryEngine. The roads are spline curves and built up with polygons matching the terrain surface. It used to work perfectly but I’ve noticed that when I’m dynamically adding the roads, which involves moving the spline curve control points around the surface of the terrain, the frame rate comes to a grinding halt.
      There’s some relatively complex processing going on each time the mouse moves - the road either side of the control point(s) being moved, is reconstructed in real time so you can position and bend the road precisely. On my previous OS, which was Win2k Pro, this worked really smoothly and in release mode there was barely any slow down in frame rate, but now it’s unusable. As part of the road reconstruction, I lock the vertex and index buffers and refill them with the new values so my question is, on windows 10 using DX9, is anyone aware of any locking issues? I’m aware that there can be contention when locking buffers dynamically but I’m locking with LOCK_DISCARD and this has never been an issue before.
      Any help would be greatly appreciated.
    • By MikhailGorobets
      I have a problem with SSAO. On left hand black area.
      Code shader:
      Texture2D<uint> texGBufferNormal : register(t0); Texture2D<float> texGBufferDepth : register(t1); Texture2D<float4> texSSAONoise : register(t2); float3 GetUV(float3 position) { float4 vp = mul(float4(position, 1.0), ViewProject); vp.xy = float2(0.5, 0.5) + float2(0.5, -0.5) * vp.xy / vp.w; return float3(vp.xy, vp.z / vp.w); } float3 GetNormal(in Texture2D<uint> texNormal, in int3 coord) { return normalize(2.0 * UnpackNormalSphermap(texNormal.Load(coord)) - 1.0); } float3 GetPosition(in Texture2D<float> texDepth, in int3 coord) { float4 position = 1.0; float2 size; texDepth.GetDimensions(size.x, size.y); position.x = 2.0 * (coord.x / size.x) - 1.0; position.y = -(2.0 * (coord.y / size.y) - 1.0); position.z = texDepth.Load(coord); position = mul(position, ViewProjectInverse); position /= position.w; return position.xyz; } float3 GetPosition(in float2 coord, float depth) { float4 position = 1.0; position.x = 2.0 * coord.x - 1.0; position.y = -(2.0 * coord.y - 1.0); position.z = depth; position = mul(position, ViewProjectInverse); position /= position.w; return position.xyz; } float DepthInvSqrt(float nonLinearDepth) { return 1 / sqrt(1.0 - nonLinearDepth); } float GetDepth(in Texture2D<float> texDepth, float2 uv) { return texGBufferDepth.Sample(samplerPoint, uv); } float GetDepth(in Texture2D<float> texDepth, int3 screenPos) { return texGBufferDepth.Load(screenPos); } float CalculateOcclusion(in float3 position, in float3 direction, in float radius, in float pixelDepth) { float3 uv = GetUV(position + radius * direction); float d1 = DepthInvSqrt(GetDepth(texGBufferDepth, uv.xy)); float d2 = DepthInvSqrt(uv.z); return step(d1 - d2, 0) * min(1.0, radius / abs(d2 - pixelDepth)); } float GetRNDTexFactor(float2 texSize) { float width; float height; texGBufferDepth.GetDimensions(width, height); return float2(width, height) / texSize; } float main(FullScreenPSIn input) : SV_TARGET0 { int3 screenPos = int3(input.Position.xy, 0); float depth = DepthInvSqrt(GetDepth(texGBufferDepth, screenPos)); float3 normal = GetNormal(texGBufferNormal, screenPos); float3 position = GetPosition(texGBufferDepth, screenPos) + normal * SSAO_NORMAL_BIAS; float3 random = normalize(2.0 * texSSAONoise.Sample(samplerNoise, input.Texcoord * GetRNDTexFactor(SSAO_RND_TEX_SIZE)).rgb - 1.0); float SSAO = 0; [unroll] for (int index = 0; index < SSAO_KERNEL_SIZE; index++) { float3 dir = reflect(SamplesKernel[index].xyz, random); SSAO += CalculateOcclusion(position, dir * sign(dot(dir, normal)), SSAO_RADIUS, depth); } return 1.0 - SSAO / SSAO_KERNEL_SIZE; }  

    • By Ike aka Dk
      Hello everyone 
      I am a programmer from Baku.
      I need a 3D Modeller for my shooter project in unity.I have 2 years Unity exp.
      Project will paid when we finish the work 
      If you interested write me on email:
  • Advertisement
  • Popular Now

  • Forum Statistics

    • Total Topics
    • Total Posts

Important Information

By using GameDev.net, you agree to our community Guidelines, Terms of Use, and Privacy Policy.

Participate in the game development conversation and more when you create an account on GameDev.net!

Sign me up!