So this was my approach and so far I'm able to send commands to a single crazyflie:
I started with this API I found on GitHub that builds MATLAB MEX functions utilizing ZMQ's C Library:
https://github.com/fagg/matlab-zmq
The documentation and commenting is pretty good. Once it's installed make sure to run their test scripts to ensure everything is built correctly. I then wrote this script to test sending inputs to the BitCraze client:
Code: Select all
%% CrazyFlie ZMQ Send - Test Script
%
% This script is intended to demonstrate how to send control inputs
% to the crazyflie using the ZMQ input socket via matlab. The functionality is
% identical to the python ZMQ input script: thrust inputs (25% - 45% - 0%) are
% sent at 1 second intervals.
%
context = zmq.core.ctx_new();
socket = zmq.core.socket(context, 'ZMQ_PUSH');
address = 'tcp://127.0.0.1:1212';
zmq.core.connect(socket, address);
cmd = struct('version',{1},'ctrl',struct('roll',0,'pitch',0,'yaw',0,'thrust',0));
cmdMessage = sprintf('{ "version": %d, "ctrl": { "roll": %f, "pitch": %f, "yaw": %f, "thrust": %f } }', cmd.version, cmd.ctrl.roll, cmd.ctrl.pitch, cmd.ctrl.yaw, cmd.ctrl.thrust);
fprintf('Sending Control inputs! \n \n \n');
%Thrust Unlock,
cmd.ctrl.thrust = 0;
cmdMessage = sprintf('{ "version": %d, "ctrl": { "roll": %f, "pitch": %f, "yaw": %f, "thrust": %f } }', cmd.version, cmd.ctrl.roll, cmd.ctrl.pitch, cmd.ctrl.yaw, cmd.ctrl.thrust);
zmq.core.send(socket, uint8(cmdMessage));
pause(0.01);
fprintf('Sending: %s \n',cmdMessage);
cmd.ctrl.thrust = 25;
cmdMessage = sprintf('{ "version": %d, "ctrl": { "roll": %f, "pitch": %f, "yaw": %f, "thrust": %f } }', cmd.version, cmd.ctrl.roll, cmd.ctrl.pitch, cmd.ctrl.yaw, cmd.ctrl.thrust);
zmq.core.send(socket, uint8(cmdMessage));
pause(1.00);
fprintf('Sending: %s \n',cmdMessage);
cmd.ctrl.thrust = 45;
cmdMessage = sprintf('{ "version": %d, "ctrl": { "roll": %f, "pitch": %f, "yaw": %f, "thrust": %f } }', cmd.version, cmd.ctrl.roll, cmd.ctrl.pitch, cmd.ctrl.yaw, cmd.ctrl.thrust);
zmq.core.send(socket, uint8(cmdMessage));
pause(1.00);
fprintf('Sending: %s \n',cmdMessage);
cmd.ctrl.thrust = 1;
cmdMessage = sprintf('{ "version": %d, "ctrl": { "roll": %f, "pitch": %f, "yaw": %f, "thrust": %f } }', cmd.version, cmd.ctrl.roll, cmd.ctrl.pitch, cmd.ctrl.yaw, cmd.ctrl.thrust);
zmq.core.send(socket, uint8(cmdMessage));
pause(1.00);
fprintf('Sending: %s \n',cmdMessage);
cmd.ctrl.thrust = 0;
cmdMessage = sprintf('{ "version": %d, "ctrl": { "roll": %f, "pitch": %f, "yaw": %f, "thrust": %f } }', cmd.version, cmd.ctrl.roll, cmd.ctrl.pitch, cmd.ctrl.yaw, cmd.ctrl.thrust);
zmq.core.send(socket, uint8(cmdMessage));
zmq.core.disconnect(socket, address);
zmq.core.close(socket);
zmq.core.ctx_shutdown(context);
zmq.core.ctx_term(context);
For SIMULINK integration, you need to utilize an initialization script to open the socket, a function block to send data to the socket, and a termination script to close the socket. Note that all of these MEX/Matlab functions will need to be declared extrinsic in the scripts and function block using the
coder.extrinsic() function. Also, you will need to handle cast the 'socket' variable as a double since the ZMQ socket function returns a uint64 and for whatever reason SIMULINK doesn't support int64/uint64 types.
Let me know if there are any questions. Good Luck.