[SOLVED] How to calculate thrust from cmdFullState

Firmware/software/electronics/mechanics
Post Reply
kato
Beginner
Posts: 13
Joined: Tue Dec 14, 2021 4:10 am

[SOLVED] How to calculate thrust from cmdFullState

Post 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.
Last edited by kato on Fri Jan 07, 2022 1:12 pm, edited 1 time in total.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: How to calculate thrust from cmdFullState

Post 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!
kato
Beginner
Posts: 13
Joined: Tue Dec 14, 2021 4:10 am

Re: How to calculate thrust from cmdFullState

Post 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!!
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: How to calculate thrust from cmdFullState

Post 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

Code: Select all

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 :

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/
kato
Beginner
Posts: 13
Joined: Tue Dec 14, 2021 4:10 am

Re: How to calculate thrust from cmdFullState

Post 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 :roll: :roll:
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: How to calculate thrust from cmdFullState

Post by kimberly »

No worries! That's what we are here for :D

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.
kato
Beginner
Posts: 13
Joined: Tue Dec 14, 2021 4:10 am

Re: How to calculate thrust from cmdFullState

Post by kato »

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!
fnafgm
Beginner
Posts: 1
Joined: Tue Mar 29, 2022 9:36 am

Re: [SOLVED] How to calculate thrust from cmdFullState

Post by fnafgm »

I have the same questions. This post resolved thoroughly my problem. Thankskrunker
Post Reply