Add status output to decoder

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}) ');']);

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:

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);
+
+<EDIT><5> // Configure input port length
 
-<EDIT><5> // Configure output ports
+<EDIT><6> // Configure output ports
 
     ssSetNumSampleTimes(S, 1);
 
@@ -87,7 +88,7 @@ static void mdlStart(SimStruct *S)
       return;
     }
 
-<EDIT><6> // encode_businfo for each message
+<EDIT><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;

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);