[SOLVED] How to calculate thrust from cmdFullState (2024)

Hi, Bitcraze
I'm Japanese college student.
Now I use CmdFullState command for Trajectory Tracking. And want to know how CmdFullState calculate input Thrust.
Correct formula is thrust = -Kp*ep - Ke*ev + mgZw + m*r_2dotT ?? from "Minimum Snap Trajectory Generation and Control for Quadrotors".

Which file is used to calculate this formula ?? I found pptraj.c in crazyswarm/crazyflie-firmware/src/modules/src and I think this file calculate trajectories.
But I couldn't find the file which use above formula. where is it ??

thanks in advance.

You are probably looking for the Mellinger controller, which can be found in controller_mellinger.c, however the output of that are attitude commands. These are translated to what is send to the 4 motors in power_distribution_stock.c.

Hope this helps!

Ah yeah the calculated trust is used in the power distribution on these lines: https://github.com/bitcraze/crazyflie-f ... c#L94-L100 . These are mostly to determine the current send to the motors in order to get these attitude rates and up thrust.

"Support to use this controller as an attitude-only controller for manual flight" means that this same controller can be used for manual flight. So that ifstament at the line that you mentioned you see

setpoint->mode.z == modeDisable 

, which means that the position control is disabled in the z-axis and that trust is directly inputted from the controller :

control->thrust = setpoint->thrust;

If it is not disabled, it will use the thrust command as calculated by the mellinger controller

btw, documentation about the stabilizer loop (state estimate, controllers, powerdistribution) can be found here: https://www.bitcraze.io/documentation/r ... o-control/

I'm sorry for reply late.
The documentation you showed me made sense to me!
So I organize my thought about cmdFullState and Thrust. Please confirm.

First, cmdFullState-msg is send to https://github.com/bitcraze/crazyflie-f ... ric.c#L299 from https://github.com/whoenig/crazyflie_ro ... r.cpp#L324 in some way.

Second, Thrust is calculated in https://github.com/bitcraze/crazyflie-f ... #L168-L171 because mode.x == modeAbs https://github.com/bitcraze/crazyflie-f ... ric.c#L321.

Finally, Thrust is inserted into control->thrust in https://github.com/bitcraze/crazyflie-f ... ger.c#L293.
I'm not so sure about this line because I couldn't find definition of mode.z. But I think so. when I use cmdFullState, mode.z != modeDisable?

sorry for my long question.
Please cooperate a little more

No worries! That's what we are here for

It seems that you got it pretty much right. And the last piece of the puzzle is that the control->thrust is send to motor distribution as I showed you before but that seems to perhaps be the topic of this forumpost so let's discuss there further.

I did notice that you are using crazyflie_ros package, which is actually deprecrated by now. The maintainer of that repository is now advising to use Crazyswarm instead, even for just one Crazyflie.

The definition of the stabilization mode can be found on these lines of stabilizer_types.h, but there is indeed no extra explanation...
So technically it is:

• modeDisable: do not respond to any velocity or position setpoints

• modeAbs: Only react to position setpoints

• modeVelocity: Only react to velocity setpoints

These meanings pretty much holds for the PID controller. But in Mellinger controller it is possible to take the full velocity and position commands in account... so the meaning is a bit off there as it is a bit special... so you really need to look in the code what these modes are use for to really understand it's meaning.

Thanks!!

No, I use Crazyswarm. I should have written the first part cmdFullState-msg is send tohttps://github.com/bitcraze/crazyflie-f ... ric.c#L299 from https://github.com/USC-ACTLab/crazyswar ... #L438-L470 in some way.

About Mellinger Controller and stabilization mode, I try to look in the code !!!

Thanks so match for answering my first topic!