diff --git a/Examples/test_messages_local.slx b/Examples/test_messages_local.slx new file mode 100644 index 0000000..bbb9f6a Binary files /dev/null and b/Examples/test_messages_local.slx differ diff --git a/Functions/create_sfun_decode.m b/Functions/create_sfun_decode.m index e78d2db..17c56ff 100644 --- a/Functions/create_sfun_decode.m +++ b/Functions/create_sfun_decode.m @@ -1,10 +1,13 @@ -function create_sfun_decode(filenames, sys_id, comp_id) +function create_sfun_decode(filenames, nmax, sys_id, comp_id) % CREATE_SFUN_DECODE: Create and compile the S-function to decode MAVLink % messages from an incoming MAVLink stream % % Inputs: % filenames: cell array of strings containing the MAVLink message header -% files for messages to be decoded +% files for messages to be decoded. +% nmax: maximum number of messages received in one iteration. This can be +% determined from the buffer size of the receiving channel. +% (default 1) % sys_id: MAVLink SYSID to be used for Simulink (default 100) % comp_id: MAVLink COMPID to be used for Simulink (default 200) % @@ -20,8 +23,9 @@ function create_sfun_decode(filenames, sys_id, comp_id) disp('--') disp('Creating decoder s-function:') -if nargin < 2, sys_id = 100; end -if nargin < 3, comp_id = 200; end +if nargin < 2, nmax = 1; end +if nargin < 3, sys_id = 100; end +if nargin < 4, comp_id = 200; end %% Create message header files @@ -86,14 +90,19 @@ function create_sfun_decode(filenames, sys_id, comp_id) fprintf(fid,'%s\n','{'); fprintf(fid,'\t%s\n','int_T *busInfo = (int_T *) ssGetUserData(S);'); fprintf(fid,'%s\n',''); +fprintf(fid,'\t%s\n','real_T *ystatus = ssGetOutputPortRealSignal(S, 0);'); +fprintf(fid,'%s\n',''); + + for i = 1:length(filenames) - fprintf(fid,'\t%s\n',['char* yvec' num2str(i-1) ' = (char *) ssGetOutputPortRealSignal(S, ' num2str(i-1) ');']); + fprintf(fid,'\t%s\n',['char* yvec' num2str(i) ' = (char *) ssGetOutputPortRealSignal(S, ' num2str(i) ');']); end fprintf(fid,'\t%s\n','switch (msg->msgid) {'); for i = 1:length(filenames) fprintf(fid,'%s\n',''); fprintf(fid,'\t\t%s\n',['case MAVLINK_MSG_ID_' upper(mavlink_msg_names{i}) ':']); - fprintf(fid,'\t\t\t%s\n',['decode_msg_' mavlink_msg_names{i} '(msg, busInfo, yvec' num2str(i-1) ', OFFSET_' upper(mavlink_msg_names{i}) ');']); + fprintf(fid,'\t\t\t%s\n',['ystatus[' num2str(i-1) '] = 1;']); + fprintf(fid,'\t\t\t%s\n',['decode_msg_' mavlink_msg_names{i} '(msg, busInfo, yvec' num2str(i) ', OFFSET_' upper(mavlink_msg_names{i}) ');']); fprintf(fid,'\t\t\t%s\n','break;'); end fprintf(fid,'\t%s\n','}'); @@ -153,17 +162,23 @@ function create_sfun_decode(filenames, sys_id, comp_id) fprintf(fout,'%s\n', '#include "sfun_decode_mavlink.h"'); case 5 + % configure input port length + fprintf(fout,'%s\n', ['ssSetInputPortVectorDimension(S, 0, ' num2str(nmax) '*MAVLINK_MAX_PACKET_LEN);']); + + case 6 % configure output ports - fprintf(fout,'\t%s\n',['if (!ssSetNumOutputPorts(S, ' num2str(i) ')) return;']); + fprintf(fout,'\t%s\n',['if (!ssSetNumOutputPorts(S, ' num2str(i+1) ')) return;']); + fprintf(fout,'%s\n',''); + fprintf(fout,'\t%s\n',['ssSetOutputPortWidth(S, 0, ' num2str(i) ');']); fprintf(fout,'%s\n',''); fprintf(fout,'\t%s\n','#if defined(MATLAB_MEX_FILE)'); fprintf(fout,'\t%s\n','if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY)'); fprintf(fout,'\t%s\n','{'); for i = 1:length(filenames) - fprintf(fout,'\t\t%s\n',['DTypeId dataTypeIdReg' num2str(i-1) ';']); - fprintf(fout,'\t\t%s\n',['ssRegisterTypeFromNamedObject(S, BUS_NAME_' upper(mavlink_msg_names{i}) ', &dataTypeIdReg' num2str(i-1) ');']); - fprintf(fout,'\t\t%s\n',['if (dataTypeIdReg' num2str(i-1) ' == INVALID_DTYPE_ID) return;']); - fprintf(fout,'\t\t%s\n',['ssSetOutputPortDataType(S, ' num2str(i-1) ', dataTypeIdReg' num2str(i-1) ');']); + fprintf(fout,'\t\t%s\n',['DTypeId dataTypeIdReg' num2str(i) ';']); + fprintf(fout,'\t\t%s\n',['ssRegisterTypeFromNamedObject(S, BUS_NAME_' upper(mavlink_msg_names{i}) ', &dataTypeIdReg' num2str(i) ');']); + fprintf(fout,'\t\t%s\n',['if (dataTypeIdReg' num2str(i) ' == INVALID_DTYPE_ID) return;']); + fprintf(fout,'\t\t%s\n',['ssSetOutputPortDataType(S, ' num2str(i) ', dataTypeIdReg' num2str(i) ');']); fprintf(fout,'%s\n',''); end fprintf(fout,'\t%s\n','}'); @@ -172,25 +187,25 @@ function create_sfun_decode(filenames, sys_id, comp_id) fprintf(fout,'%s\n',''); for i = 1:length(filenames) - fprintf(fout,'\t%s\n',['ssSetBusOutputObjectName(S, ' num2str(i-1) ', (void *) BUS_NAME_' upper(mavlink_msg_names{i}) ');']); + fprintf(fout,'\t%s\n',['ssSetBusOutputObjectName(S, ' num2str(i) ', (void *) BUS_NAME_' upper(mavlink_msg_names{i}) ');']); end fprintf(fout,'%s\n',''); for i = 1:length(filenames) - fprintf(fout,'\t%s\n',['ssSetOutputPortWidth(S, ' num2str(i-1) ', 1);']); + fprintf(fout,'\t%s\n',['ssSetOutputPortWidth(S, ' num2str(i) ', 1);']); end fprintf(fout,'%s\n',''); for i = 1:length(filenames) - fprintf(fout,'\t%s\n',['ssSetBusOutputAsStruct(S, ' num2str(i-1) ', 1);']); + fprintf(fout,'\t%s\n',['ssSetBusOutputAsStruct(S, ' num2str(i) ', 1);']); end fprintf(fout,'%s\n',''); for i = 1:length(filenames) - fprintf(fout,'\t%s\n',['ssSetOutputPortBusMode(S, ' num2str(i-1) ', SL_BUS_MODE);']); + fprintf(fout,'\t%s\n',['ssSetOutputPortBusMode(S, ' num2str(i) ', SL_BUS_MODE);']); end - case 6 + case 7 % encode_businfo for each message for i = 1:length(filenames) fprintf(fout,'\t%s\n',['encode_businfo_' mavlink_msg_names{i} '(S, busInfo, OFFSET_' upper(mavlink_msg_names{i}) ');']); diff --git a/Images/example_decode.png b/Images/example_decode.png new file mode 100644 index 0000000..0391ad9 Binary files /dev/null and b/Images/example_decode.png differ diff --git a/Images/example_encode.png b/Images/example_encode.png new file mode 100644 index 0000000..989ef28 Binary files /dev/null and b/Images/example_encode.png differ diff --git a/README.md b/README.md index e272fd3..51cc16c 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ # Simulink-MAVLink **MAVLink communication support for Simulink** -![Encode Image](https://raw.githubusercontent.com/aditya00j/simulink_mavlink/master/images/example_encode.PNG) -![Decode Image](https://raw.githubusercontent.com/aditya00j/simulink_mavlink/master/images/example_decode.PNG) +![Encode Image](Images/example_encode.PNG) +![Decode Image](Images/example_decode.PNG) ___ ## What is Simulink-MAVLink? @@ -20,27 +20,23 @@ This library was built and tested on Matlab R2017a. ## Getting Started -### Create Message Busses and S-Functions +### Step 1: Create Message Busses and S-Functions 1. Use the script `setup.m` to choose which MAVLink messages need to be encoded/decoded for your application. 2. Running the script will create the message busses, write the header and source files for the message encoder and decoder functions, and finally compile the s-functions using the `mex` command. Assuming all the options are specified correctly and the mex compilation is working, the compiled S-functions are created in the `Generated` folder. -For each message to be encoded, a separate S-function is created, that accepts the message bus as input, and creates a uint8 array of length `MAVLINK_MAX_PACKET_LEN` (= 280 for MAVLink v2.0). On the other hand, for decoding the received MAVLink data, there is only a single S-function, which accepts a uint8 array of length `MAVLINK_MAX_PACKET_LEN`, and decodess it into individual message busses. +For each message to be encoded, a separate S-function is created, that accepts the message bus as input, and creates a uint8 array of length `MAVLINK_MAX_PACKET_LEN` (= 280 for MAVLink v2.0). +On the other hand, for decoding the received MAVLink data, there is only a single S-function, which accepts a uint8 array of length `NMAX * MAVLINK_MAX_PACKET_LEN`, where `NMAX` is a user-specified maximum number of messages to be decoded in one iteration. This can be determined from the amount of buffer used in the receiving channel. The decoder S-function has `n+1` output ports, where `n` is the number of messages selected for decoding. The first output is a vector of length `n`, which indicates a 1/0 received status for each message received. If a message is received and decoded successfully in a given iteration, the status for that message is set to 1, otherwise 0. The output ports 2 onwards are the message busses, into which the decoded data is unpacked. -### Use S-Functions in Simulink to Test Local Encoding/Decoding -To verify that the encoding/decoding works correctly, open the Simulink model `Examples\test_attitude.slx`. This model simply packs arbitrary data into the Attitude message bus. Run the model and verify that the data in busses `sent_bus` and `recd_bus` matches. +### Step 2: Use S-Functions in Simulink to Test Local Encoding/Decoding -Note the following points while running the model: +To verify that the encoding/decoding works correctly, open the Simulink model `Examples\test_messages_local.slx`. This model packs the messages 'ping' and 'param_set' into a single uint8 stream using their encoder functions, and then decodes this stream using the decoder function. Note the following points while running the model: -* All the message busses (`mavlink_attitude_t` in this case) must be loaded into the base workspace before running the model. +* All the message busses must be loaded into the base workspace before running the model. * The signal on the input port of an encoder S-function must be a non-virtual bus, and all the individual signal types for the bus input must be set correctly. -### Use S-Functions in Simulink to Exchange MAVLink Data on TCP - -The Simulink model `Examples\test_attitude_tcp.slx` shows how the bit streams can be sent/received over TCP to communicate with PX4 Autopilot. Example of sending and receiving data over TCP/IP in Simulink is explained [here](https://www.mathworks.com/help/instrument/send-and-receive-data-over-a-tcpip-network.html). Once you've started an echo server on port 4560 in Matlab, you can run the Simulink model and verify that the data in busses `msg_sent` and `msg_recd` matches. - ## General Principles Following general principles were used in formulating this library: diff --git a/Templates/sfun_decode_mavlink_template.cpp b/Templates/sfun_decode_mavlink_template.cpp index fcb4f25..55c3c35 100644 --- a/Templates/sfun_decode_mavlink_template.cpp +++ b/Templates/sfun_decode_mavlink_template.cpp @@ -42,9 +42,10 @@ static void mdlInitializeSizes(SimStruct *S) ssSetInputPortDirectFeedThrough(S, 0, 1); ssSetInputPortRequiredContiguous(S, 0, 1); ssSetInputPortDataType(S, 0, SS_UINT8); - ssSetInputPortVectorDimension(S, 0, MAVLINK_MAX_PACKET_LEN); + +<5> // Configure input port length -<5> // Configure output ports +<6> // Configure output ports ssSetNumSampleTimes(S, 1); @@ -87,7 +88,7 @@ static void mdlStart(SimStruct *S) return; } -<6> // encode_businfo for each message +<7> // encode_businfo for each message ssSetUserData(S, busInfo); } /* end mdlStart */ @@ -104,6 +105,13 @@ static void mdlOutputs(SimStruct *S, int_T tid) int_T len_uvec = ssGetInputPortWidth(S, 0); const uint8_T* uvec = (uint8_T*) ssGetInputPortSignal(S, 0); + + // Set the read status to zero + int_T len_ystatus = ssGetOutputPortWidth(S, 0); + real_T *ystatus = ssGetOutputPortRealSignal(S, 0); + for (int yidx = 0; yidx < len_ystatus; yidx++) { + ystatus[yidx] = 0; + } mavlink_message_t msg; mavlink_status_t status; diff --git a/setup.m b/setup.m index ea69fa6..9e95538 100644 --- a/setup.m +++ b/setup.m @@ -2,11 +2,14 @@ % MAVLink messages with PX4 according to the Simulator MAVLink API: % https://dev.px4.io/master/en/simulation/index.html -% In this example, a single message, attitude, is sent from one port from -% Simulink and received and decoded on another port. This process tests -% that the MAVLink encoding/decoding works correctly. You can then change -% the options below to include whichever messages need to be -% encoded/decoded for your application. +% Use this file to create the busses and S-functions for the messages to be +% encoded/decoded. + +% In this example, two sample messages ('ping' and 'param_set') are encoded +% and subsequently decoded. You can change the options below to include +% whichever messages need to be encoded/decoded for your application. +% Actually sending/receiving the data through TCP/UDP/UART can be done +% using the communication tools available in Simulink. %% Set up paths and configuration options @@ -17,15 +20,19 @@ % (DO NOT use relative path) mavlink_path = '/Users/aditya/Documents/PX4/mavlink/include/mavlink/v2.0/common'; +% Maximum number of messages to receive in one iteration +nmax = 2; % Select messages to send messages_to_send = { - 'attitude'; + 'ping'; + 'param_set'; }; % Select messages to receive messages_to_receive = { - 'attitude'; + 'ping'; + 'param_set'; }; @@ -42,4 +49,4 @@ for i = 1:length(messages_to_receive) filenames{i} = strcat(mavlink_path, '/mavlink_msg_', messages_to_receive{i}, '.h'); end -create_sfun_decode(filenames); +create_sfun_decode(filenames, nmax);