[SOLVED] Control of two Crazyflies

Firmware/software/electronics/mechanics
Post Reply
lara
Beginner
Posts: 5
Joined: Sun Jan 05, 2020 6:57 pm

[SOLVED] Control of two Crazyflies

Post by lara »

Hello everybody,

I am a student from Germany. At our university, we use a motion capture system to record the movement of the Crazyflie 2.1. At present, we control one Crazyflie via cfzmq server and a Crazyradio PA using our program written in MATLAB. We push roll, pitch, yaw and thrust commands as described in https://wiki.bitcraze.io/doc:crazyflie: ... fzmq:index.

Now, we want to control two Crazyflies by using one Crazyradio PA, but I have problems with the communication. How can I control two Crazyflies via cfzmq server and how can I connect a Crazyradio PA with two Crazyflies?
Do I need a second cfzmq server with different ports open as described in viewtopic.php?t=1943? If so, how could I realize that? Unfortunately, reiinakano seems to be inactive.

Thank you in advance for your help!
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Control of two Crazyflies

Post by arnaud »

Unfortunately cfzmq was not designed to control more than one Crazyflie and it has never been updated to do so.

If keeping matlab is a hard requirement for you I can see 4 solutions to go forward:
- Running a second cfzmq client on different ports using a different Crazyradio. To use a second Crazyradio all you have to do is to change the first digit of the connection URI (for example radio://0/80/2M/E7E7E7E701 for the first one and radio://1/70/2M/E7E7E7E702 for the second). This should require minimal change of the cfzmq client.
- Updating cfzmq to be able to connect more than one Crazyflie. This is a much bigger job!
- Using ROS: there is a robotic toolbox for Matlab that allows using ROS and the Crazyflie ROS driver can connect many Crazyflies using one or many radios.
- Making a minimal python program that connects both Crazyflie and expose a ZMQ socket accepting setpoints. If you have some experience with python this should be fairly quick to write and debug starting from the example available in the Crazyflie lib example folder.

I hope this helps a bit clarifying the possible forward path. There might be other solutions, but this is the main one I can think about. Do not hesitate if you need more precision about how to got forward with any of them.
lara
Beginner
Posts: 5
Joined: Sun Jan 05, 2020 6:57 pm

Re: Control of two Crazyflies

Post by lara »

Thank you for your reply!

possibility 1)
I have already tried to install a second cfzmq client, but it does not work.
How can I simultaneously call two cfzmq clients?
What has to be changed in the code of the cfzmq client?
As I understand it, two cfzmq clients must run in parallel to connect and control two Crazyflies at the same time.
Is the communication reliable in this way and what kind of latency is there?

possibility 2)
This is a big job and I am not familiar with python.

possibility 3)
I have already thought about the Crazyflie ROS project of Wolfgang Hoenig but the ROS project can only be used under Linux and our computer is running Windows 10.

possibility 4)
Unfortunately, I have not much experience with python. I have looked at the example "multiramp". It connects two Crazyflies and sends both the same commands.
How can I send different commands? How can I also connect two Crazyflies and control them individually? As I understand, with this method there is no need for the cfzmq. If so, how can I realize the communication between matlab and python?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Control of two Crazyflies

Post by arnaud »

It sounds like the possibility 1 is the best for your then.

The only change to make to be able to launch 2 cfzmq client is to change the ports used by the ZMQ sockets. I pushed a change to cfzmq that adds a parameter to change the base-port: https://github.com/bitcraze/crazyflie-c ... issues/385.

So, if you pull the latest version of the crazyflie-clients-python project, you will be able to launch cfzmq with a -p argument to change the zmq port. For example you can launch one with "cfzmq -p 2000" and a second one with "cfczq -p 3000". Then in matlab, you can use one for each Crazyflie.

You need to use 2 Crazyradios and to connect your first Crazyflie with an URI starting by "radio://0/..." and the second with "radio://1/....". This way each cfzmq will use its own radio. Since you are using one radio per Crazyflie, there should be no change in latency.

As a side note, to clarify my 4th possibility, I was suggesting you to connect 2 Crazyflie and implement ZMQ communication by yourself, kind of making a special-purpose cfzmq client. Since you do not have much experience in Python, using cfzmq directly might be better though.
lara
Beginner
Posts: 5
Joined: Sun Jan 05, 2020 6:57 pm

Re: Control of two Crazyflies

Post by lara »

Thank you very much for your help! Now, I can launch two cfzmqs.
After receiving the second Crazyradio, I tried to connect the two Crazyflies, but unfortunately, I got problems with the ZMQ-server.

I am using the following library https://github.com/fagg/matlab-zmq to establish a connection between MATLAB and ZMQ. The MATLAB functions used to communicate with cfzmq (which I has from https://github.com/MOHAMMADZAHD93/CrzayMatlab ) are as follows:

Code: Select all

% function to start the ZMQ-client and to create sockets
function [socketcomm, socketcont, socketcomm2, socketcont2] = ZMQSERVER()

system('start cmd.exe @cmd /k "cfzmq -p 2000"')
% Command socket: 
contextcomm = zmq.core.ctx_new();
socketcomm = zmq.core.socket(contextcomm, 'ZMQ_REQ');
addresscomm = 'tcp://127.0.0.1:2000';
zmq.core.connect(socketcomm, addresscomm);
% Log socket:
contextlog = zmq.core.ctx_new();
socketlog  = zmq.core.socket(contextlog, 'ZMQ_SUB');
addresslog = 'tcp://127.0.0.1:2001';
zmq.core.connect(socketlog, addresslog);
zmq.core.setsockopt(socketlog,'ZMQ_SUBSCRIBE','');
% Param Socket:
contextparam = zmq.core.ctx_new();
socketparam  = zmq.core.socket(contextparam, 'ZMQ_SUB');
addressparam = 'tcp://127.0.0.1:2002';
zmq.core.connect(socketparam, addressparam);
zmq.core.setsockopt(socketparam,'ZMQ_SUBSCRIBE','');
% Connection socket:
contextconn = zmq.core.ctx_new();
socketconn  = zmq.core.socket(contextconn, 'ZMQ_SUB');
addressconn = 'tcp://127.0.0.1:2003';
zmq.core.connect(socketconn, addressconn);
zmq.core.setsockopt(socketconn,'ZMQ_SUBSCRIBE','');
% Control socket:
contextcont = zmq.core.ctx_new();
socketcont  = zmq.core.socket(contextcont, 'ZMQ_PUSH');
addresscont = 'tcp://127.0.0.1:2004';
zmq.core.connect(socketcont, addresscont);

% second Crazyflie
system('start cmd.exe @cmd /k "cfzmq -p 3000"')
% Command socket: 
contextcomm2 = zmq.core.ctx_new();
socketcomm2 = zmq.core.socket(contextcomm2, 'ZMQ_REQ');
addresscomm2 = 'tcp://127.0.0.1:3000';
zmq.core.connect(socketcomm2, addresscomm2);
% Log socket:
contextlog2 = zmq.core.ctx_new();
socketlog2  = zmq.core.socket(contextlog2, 'ZMQ_SUB');
addresslog2 = 'tcp://127.0.0.1:3001';
zmq.core.connect(socketlog2, addresslog2);
zmq.core.setsockopt(socketlog2,'ZMQ_SUBSCRIBE','');
% Param Socket:
contextparam2 = zmq.core.ctx_new();
socketparam2  = zmq.core.socket(contextparam2, 'ZMQ_SUB');
addressparam2 = 'tcp://127.0.0.1:3002';
zmq.core.connect(socketparam2, addressparam2);
zmq.core.setsockopt(socketparam2,'ZMQ_SUBSCRIBE','');
% Connection socket:
contextconn2 = zmq.core.ctx_new();
socketconn2  = zmq.core.socket(contextconn2, 'ZMQ_SUB');
addressconn2 = 'tcp://127.0.0.1:3003';
zmq.core.connect(socketconn2, addressconn2);
zmq.core.setsockopt(socketconn2,'ZMQ_SUBSCRIBE','');
% Control socket:
contextcont2 = zmq.core.ctx_new();
socketcont2  = zmq.core.socket(contextcont2, 'ZMQ_PUSH');
addresscont2 = 'tcp://127.0.0.1:3004';
zmq.core.connect(socketcont2, addresscont2);
end

Code: Select all

% function to connect
function result = ConnectCrazyzmq(socketcomm, uri)
connectcmd=['{"version": 1,"cmd": "connect","uri": "',uri,'"}'];
zmq.core.send(socketcomm, uint8(connectcmd));
pause(5)
result=char(zmq.core.recv(socketcomm,40000,'ZMQ_DONTWAIT'));
end

Code: Select all

% function to send commands
function sendcommandsCrazy(socketcont, Thrust, roll, pitch, yaw)
Msg=['{"version": 1,"roll": ',num2str(roll),',"pitch": ',num2str(pitch),...
',"yaw": ',num2str(yaw),',"thrust": ',num2str(Thrust),'}'];
zmq.core.send(socketcont, uint8(Msg));
end
Every time I get the error message "error using recv Resource temporarily unavailable" in MATLAB. This comes from the function "ConnectCrazyzmq()" in the line with "zmq.core.recv(...)". I tried many different values for 40000 and I also vary the pause, but unfortunately without success so far. I would be very pleased, if someone could help me solving this problem.

I use the URI "radio://0/80/2M" for the first and the URI"radio://1/100/2M" for the second Crazyflie. Is this correct?
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Control of two Crazyflies

Post by kimberly »

Yes, those uries are indeed correct!

These are unfortunately errors that it a bit difficult to help with since we don't work with matlab ourselves. But you tried the cfzmq directly from the terminal right? so that is working properly.

Currently I wouldn't know exactly how to fix this for you, but it would be good if you can make a minimal working example for yourselve that only checks the connection. It might also be an issue on the matlab side of things that it is not detecting the cfradio. Once Arnaud is back next week I will ask if he has any ideas on this as well.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Control of two Crazyflies

Post by arnaud »

I think the problem comes from 'ZMQ_NOWAIT'. According to https://stackoverflow.com/questions/143 ... nd-command this error just means that there was nothing to read when you called the receive function and since you asked it not to wait it returns directly.

The best would be to get your script working well with one Crazyflie and then adding the second one. Though, everything does look correct in the way you launch and uses the two cfzmq.
lara
Beginner
Posts: 5
Joined: Sun Jan 05, 2020 6:57 pm

Re: Control of two Crazyflies

Post by lara »

It works now. I used the URIs mentioned above and instead of 40000 and a pause of 5 seconds I entered 20000 and a pause of 3 seconds.

Thank you very much for your help!
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Control of two Crazyflies

Post by kimberly »

Great! let us know if you have any more problems. For now I will mark this thread solved :)

Are you interested to write instructions about using Matlab with the Crazyflie? Either as a blogpost/ forum thread or add something to the doc? This sounds like more people would be interested in, since we get this request more often.
lara
Beginner
Posts: 5
Joined: Sun Jan 05, 2020 6:57 pm

Re: [SOLVED] Control of two Crazyflies

Post by lara »

I am currently writing my thesis, so unfortunately I don't have that much time at the moment. Thanks for the offer. That would be very interesting for me. I would gladly help the others who also want to work with Matlab. We can get in contact with each other after my thesis (I think in April). For concrete questions I am available with pleasure.
Post Reply