Position Control

[cygaro]

Hi everyone.
I'm trying to develop a reasonable model of the crazyflie on vrep. At this moment I have the drone model with mass and inertia found on a paper about model identification of the drone. I'm trying to use the same control used in the firmware by writing it on a script associated to the drone model, but some points of it are not really clear to me.
Note that what I want is to use a reference point to be tracked by the drone. So the input is the position of that point (waypoint).
If I'm wright the logic is the following:

Stage1 - position control:
Here we have a PID which errors are the relative distances between the drone and the waypoint.
Stage2 - velocity control:
The output value of the previous PID are used as desired values for stage2 PID which works on velocities. As an example if the output of the previous PID on z position is called uZ, at this stage the error feeded to the PID is: uZ-velocity.z
After this stage we have a raw value of thrust (output of stage2 PID along z direction) which would be enough to control the vertical position. It works really fine in my simulations, but what comes next is not clear to me.
Stage3 - attitude control:
x and y output of previous step are used as desired values for roll and pitch in the attitude PID controller (?).
Stage4 - attitude rate control:
x and y output of previous step are used as desired values for roll rate and pitch rate in this PID, the output of which is somehow a value of thrust to properly add to the value founded at stage2.

I think I'm not understanding well how the x,y control works, because it is really not doing the job. Also there may be some error in my code, but I need first to be sure to understand properly the way the position control as present on the firmware.

Are those steps right? Can you confirm that all the value used are in meters and degrees (rather then rad)?

Thank you,
Giuseppe

[arnaud]

Hi Cygaro,

Your analysis looks good to me. And yes we use degree and meters (we should certainly convert to radian at some point).

/Arnaud