Page 1 of 1
[SOLVED] How to calculate thrust from cmdFullState
Posted: Tue Dec 14, 2021 4:15 am
by kato
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.
Re: How to calculate thrust from cmdFullState
Posted: Wed Dec 15, 2021 12:37 pm
by kimberly
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!
Re: How to calculate thrust from cmdFullState
Posted: Fri Dec 17, 2021 1:22 am
by kato
Thank you for your reply.
That flies are what I'm looking for. And as you said, controller_mellinger.c says ""Support to use this controller as an attitude-only controller for manual flight"" in line 35.
But Thrust is calculated at line 293. Does the Thrust calculated in that file isn't used in power_distribution_stock.c ?
Sorry for my poor English. Thanks!!
Re: How to calculate thrust from cmdFullState
Posted: Fri Dec 17, 2021 9:13 am
by kimberly
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
, which means that the position control is disabled in the z-axis and that trust is directly inputted from the controller :
Code: Select all
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/
Re: How to calculate thrust from cmdFullState
Posted: Mon Dec 20, 2021 12:58 pm
by kato
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
Re: How to calculate thrust from cmdFullState
Posted: Wed Dec 22, 2021 8:38 am
by kimberly
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.
Re: How to calculate thrust from cmdFullState
Posted: Tue Dec 28, 2021 5:04 am
by kato
Thanks!!
No, I use Crazyswarm. I should have written the first part cmdFullState-msg is send to
https://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!
Re: [SOLVED] How to calculate thrust from cmdFullState
Posted: Tue Mar 29, 2022 9:44 am
by fnafgm
I have the same questions. This post resolved thoroughly my problem. Thanks
krunker