High-level to position control - where in the firmware code?

[matejkarasek]

Hi,

I am trying to fly a non-standard drone platform autonomously, using the Roadrunner and LPS:
https://www.youtube.com/watch?v=ybk1CWp ... e=youtu.be
(the video is recorded with a different autopilot)

I was able to adjust the power distribution and low-level control loops (attitude and rate) so can fly manually without problems.
Position estimation using the LPS is also working (although not when using external Rx via BigQuad deck, but already looking for a solution in another thread).

To fly autonomously, I still need to tune the position control gains (I know where), but first I need to adjust the take-off and landing routines, and that is where I am lost. For example, I would like to start the motors with a low rpm, wait a second, and only then take off.
I would also like to use the assisted mode for the initial tests, but for now, haven't found which part of the firmware is used in that case.

So what remains unclear to me is what part of code is converting the high-level commands (take off, land, goto) into the inputs to the position controller.
I found the planner, but haven't yet figured out how things are linked...

Any help would be greatly appreciated!

Matej

[kimberly]

Hi!

So the planner output is being transferred into setpoints for it's durration, which happens here: https://github.com/bitcraze/crazyflie-f ... vel.c#L229. These setpoints are being retrieved by the commander.c in here: https://github.com/bitcraze/crazyflie-f ... nder.c#L89, which is retrieved by the stabilzer.c in here https://github.com/bitcraze/crazyflie-f ... zer.c#L276. And there is where all the controlling begins.

Is this in the direction you are talking about?

I must say it is not super clear either... We will probably need to write some more specific documentation on how the whole control structure works to be honest since more people (and me ) are getting lost in this.

[matejkarasek]

Thanks a lot, this is already very helpful.

Not everything is clear yet but got enough ideas on where to look further
Some documentation of the control structure would definitely be great


I see that the high-level commands are sent from python via packets. On the firmware side, how are these processed and linked to the controller?

And could you also point me to the code running in the case of the assist mode (position)?

Thanks!

[kimberly]

Hopefully this quarter we can make documentation about the controller structure indeed

So the packages come through the radio link to crtp.c, which puts the HLC message in the ctrp que. This is then read out by the ctrp_commander_high_level.c here: https://github.com/bitcraze/crazyflie-f ... vel.c#L291. From there the magic happens as I tried to explain in the last post .

So the assist mode is being handled in ctrp_commander_ctrp.c as altHoldMode or posHoldMode (which are variables that are controlled as parameters. If these are enabled, the mode for x y z position arr put into velocity mode, which means is that the input through the controller is considered a velocity reference (not as angle references as with the attitude mode).

Hope this makes sense!

[matejkarasek]

Yes, it makes sense, thanks!

I want to start the engines before the take-off and wait a bit till they really start (BL motors do not start immediately, unlike the brushed DCs on CF2...)

Looking further into the firmware, I see that the takeoff and landing routines are simply implemented as trajectories, so it is not straightforward to implement it there.
But I also got a bit more familiar with the python API, and I see now that I should be able to achieve this using the function 'send_setpoint(roll, pitch, yaw, thrust)'.

I started with the initial_position.py example and added this code at the beginning of run_sequence https://github.com/bitcraze/crazyflie-l ... on.py#L121

Code: Select all

cf = scf.cf
print('Starting engines')
    for i in range(500):
    cf.commander.send_setpoint(0.0, 0.0, 0, 15000)
    time.sleep(0.01)

However, no matter what thrust values I use (tried up to 60000), the motors do not start.
After that, the CF takes off as it should.

Looking into the firmware, I suspect that in crtpCommanderRpytDecodeSetpoint() https://github.com/bitcraze/crazyflie-f ... pyt.c#L119, the thrustLocked remains true, so nothing happens.
But maybe it's something else, do I need to set some parameters beforehand if I want to set rpyt directly?

[kimberly]

Hmm... it is a bit unclear to see what is going on.... it seems like this should work from a quick look at it....

Have you tried out the ramp.py example ? https://github.com/bitcraze/crazyflie-l ... es/ramp.py. It is a script that will make the motors of the crazyflie run faster and faster (meant for keeping the crazyflie in your hand]. Here you would need to probably change this line , so that you will only give a constant thrust.

Let us know if this works

[kimberly]

So, in ramp.py, you first have to send a zero setpoint to unlock the thrust protection (https://github.com/bitcraze/crazyflie-l ... amp.py#L89). It seems that that is missing in your initial script. Hope this is the culprit!

[matejkarasek]

I missed the ramp.py example, that is exactly what I needed
I will try in the afternoon, but it is very likely it is just the zero setpoint that I need to add, thanks!