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.
[SOLVED] How to calculate thrust from cmdFullState
[SOLVED] How to calculate thrust from cmdFullState
Last edited by kato on Fri Jan 07, 2022 1:12 pm, edited 1 time in total.
Re: How to calculate thrust from cmdFullState
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!
Hope this helps!
Re: How to calculate thrust from cmdFullState
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!!
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
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 : 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/
"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
Code: Select all
setpoint->mode.z == modeDisable
Code: Select all
control->thrust = setpoint->thrust;
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
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
![:-/ :roll:](./images/smilies/icon_rolleyes.gif)
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
![:-/ :roll:](./images/smilies/icon_rolleyes.gif)
![:-/ :roll:](./images/smilies/icon_rolleyes.gif)
Re: How to calculate thrust from cmdFullState
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:
![:-D :D](./images/smilies/icon_e_biggrin.gif)
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
Re: How to calculate thrust from cmdFullState
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!
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!
Re: [SOLVED] How to calculate thrust from cmdFullState
I have the same questions. This post resolved thoroughly my problem. Thankskrunker