%% Use Handwritten Code to Integrate C++ Messages with POSIX
% This example shows how to integrate generated C++  
% code with POSIX to facilitate message 
% communication. The example application is a 
% multisensor positioning system designed to estimate the position of 
% a vehicle. The multisensor positioning system is composed of two 
% components: sensors (multiple models) and an estimation model. The outlined 
% workflow shows how to prepare each component, 
% generate code, and integrate the generated code to pass messages between 
% system components. This example assumes you are familiar with using
% messages in the Simulink environment. For more information, 
% see <docid:ecoder_ug#mw_b3d60f69-2274-4ed5-81df-b0842cd23062>.
%
% Copyright 2020 The MathWorks, Inc.

%% Sensor Models
% The first component in the multisensor positioning system is the 
% sensors. The positioning system uses an accelometer and a GPS sensor to estimate the vehicle 
% position. Each sensor model is composed of a Function block, Send block, and 
% Output port. Each sensor model sends messages to the estimation model by 
% using POSIX messages.
%%
% *Prepare Models*
%
% Open the sensor models |rtwdemo_accel_send| and |rtwdemo_gps_send|.
%
%%
% 
% <<../accel_model.png>>
%
%%
% 
% <<../gps_model.png>>
%
%%
% For each model, perform these steps:
%
% # In the Apps Gallery, click *Embedded Coder*. 
% # On the toolstrip, set the *Stop time* to |100| seconds. The stop time defines
% the duration of the data collection when estimating the vehicle position.
% # In the model, click to open the Function block. In the Block Parameters,
% set the *File name* to the MAT-file for the model. Set the *Sample time* to |0.01|.
%%
% *Generate Code*
%
% 1. For each sensor model, in the Configuration Parameters dialog box, set
% these parameters:
% 
% * On the *Code Generation* pane, set *Language* to |C++|. Under Advanced parameters, set *Build
% configuration* to |Specify| and add |-lrt| to the *C++ Linker*. 
% * On the *Interface* pane , set *Code interface packaging* to |C++ class| 
% and *Multi-instance code error diagnostic* to |none|. Under Advanced parameters, select |MAT-file logging|.
%
% 2. Generate code. On the *C++ Code* tab, click *Build*.
%%
% *Integrate Code*
%
% To integrate the generated C++ code from each sensor model with POSIX,
% hand write the implementation of the send class and generated main program (or create
% your own). This example provides a customized main program with an implemented
% send class. To integrate the code from each sensor model, replace the generated 
% main file with the customized main file.
%
% Review the customized main file for each sensor:
%  
% 1. The customized main file for each sensor is implemented in the same way.
% As an example, open the customized main file
% |rtwdemo_accel_send_ert_main_customized.cpp|.
%
% 2. Review the implemented send class:
% 
% * The send class function, |SendData|, calls into the
% POSIX API (|mq_send|) to send messages.
% * To provide access to a message queue, a POSIX message
% queue is added as a class variable and a set method is implemented to
% set the queue.
% 
%  class accelSendModelClassSendData_real_T : public SendData_real_T{
%       mqd_t msgQueue;
%
%       public:
%       // Use POSIX to send messages
%       void SendData(const real_T* data, int32_T length, int32_T* status)
%       {
%           unsigned int priority = 1;
%           *status = mq_send(msgQueue, (char*)data, length, priority);
%       }
%
%       // Set method for class POSIX queue
%       void SetMQ(mqd_t mq)
%       {
%           msgQueue = mq;
%       }
%   }; 
%
% After the send class implementation, an 
% instance of the send class (a send object) is created. The send object
% is then attached to the sensor model to enable the sensor to send  
% messages to the estimate model.
%
%  // Create send object
%  static accelSendModelClassSendData_real_T Out1SendData_arg; 
%
%  // Attach send object to sensor model to send messages
%  static accelSendModelClass rtObj( Out1SendData_arg);
% 
% 3. Review the added POSIX queue behavior functions. The main
% function uses these functions to open and close the POSIX queue.
%
%  mqd_t openMQ(const char *name, int flags, int maxmsg, int msgsize)
%   {
%       struct mq_attr attr;
%       attr.mq_flags = 0;
%       attr.mq_maxmsg = maxmsg;
%       attr.mq_msgsize = msgsize;
%
%       mqd_t msgQueue = mq_open(name, flags, 0664, &attr);
%
%       if (msgQueue < 0) {
%           printf("mq_open failed\n");
%           exit(1);
%       }
%
%       return msgQueue;
%   }
%
%  void closeMQ(mqd_t msgQueue)
%   {
%       mq_close(msgQueue);
%   }
%
% 4. Review the main function. The implemented main 
% function uses the previously defined class and queue functions to send 
% messages and to manage the POSIX message queue. The main function performs
% the following:
%
% * Opens a POSIX message queue.
% * Sends messages.
% * Closes the POSIX queue.
%
%  int_T main(int_T argc, const char *argv[])
%   {
%       // Unused arguments
%       (void)(argc);
%       (void)(argv);
%
%       // Initialize model
%       rtObj.initialize();
%  
%       // Open POSIX message queue
%       mqd_t msgQueue = openMQ("/PosixMQ_accel", O_CREAT | O_WRONLY, 2, 16);
%       Out1SendData_arg.SetMQ(msgQueue);
%
%       // Send messages
%       while (rtmGetErrorStatus(rtObj.getRTM()) == (NULL)) {
%       rt_OneStep();
%       }
%
%       // Matfile logging
%       rt_StopDataLogging(MATFILE, rtObj.getRTM()->rtwLogInfo);
%
%       // Disable rt_OneStep() here
%
%       // Close POSIX message queue
%       closeMQ(msgQueue);
% 
%       return 0;
%   }
%% Estimate Model
% The second component is the estimate model. The estimate model receives
% messages that have input data from the sensor models and calculates the estimated 
% position of the vehicle.
%% 
% *Prepare Model*
%
% Open the model |rtwdemo_pos_estimate|.
%%
% 
% <<../estimate_model.png>>
% 
%%
% # In the Apps Gallery, click *Embedded Coder*. 
% # On the toolstrip, set the *Stop time* to |100| seconds.
% # In the model, click to open each inport. In the Block Parameters,
% set *Data type* to |double|, *Port dimensions* to |3|, and *Sample time* to |0.01|.
% # Click to open each Message Receive block. Clear the *Use internal
% queue* parameter to enable the model to use a POSIX message queue.
% # Click to open the Function block. This block implements the estimation algorithm.
%%
% *Generate Code*
%
% 1. In the Configuration Parameters dialog box, set
% these parameters:
% 
% * On the *Code Generation* pane, set *Language* to |C++|. Under Advanced parameters, set *Build
% configuration* to |Specify| and add |-lrt| to the *C++ Linker*.
% * On the *Interface* pane, set *Code interface packaging* to |C++ class| and set *Multi-instance code error diagnostic* to |none|.
% Under Advanced parameters, select |MAT-file logging|.
%
% 2. Generate code. On the *C++ Code* tab, click *Build*.
%%
% *Integrate Code*
%
% To integrate the generated C++ code from the estimate model with POSIX, hand
% write the implementation of the receive class and generated main 
% program (or create your own). The estimate model
% receives messages from the sensor models and calculates an estimated position of the vehicle. 
% This examples provides a customized main program with an implemented
% receive class. To integrate the code from the estimate model, replace
% the generated main file with the customized main file.
%
% Review the customized main file:
%
% 1. Open the customized main file |rtwdemo_pos_estimate_ert_main_customized.cpp|.
%
% 2. Review the implemented receive class:
%
% * The receive class function, |RecvData|, calls into the
% POSIX API (|mq_receive|) to receive messages.
% * To provide access to a message queue, a POSIX message
% queue is added as a class variable and a set method is implemented to
% set the queue.
%
%  class positionEstimateModelClassRecvData_real_T : public RecvData_real_T{
%       mqd_t msgQueue;
%
%       public:
%       // Uses POSIX API mq_receive to receive messages
%       void RecvData(real_T* data, int32_T length, int32_T* status)
%       {
%       // Add receive data logic here 
%       unsigned int priority = 1;
%       *status = mq_receive(msgQueue, (char *)data, length, &priority);
%       }
%
%       // Set method for class POSIX queue
%       void SetMQ(mqd_t mq)
%       {
%           msgQueue = mq;
%       }
%    };
%
% After the receive class implementation, an instance of the 
% receive class (a receive object) is created for each sensor model. The
% receive objects are then attached to the estimate model to enable the model 
% to receive messages from each sensor.
%
%  // Create receive objects
%  static positionEstimateModelClassRecvData_real_T In1RecvData_arg;
%  static positionEstimateModelClassRecvData_real_T In2RecvData_arg;
%
%  // Attach receive objects to estimate model to receive messages
%  static positionEstimateModelClass rtObj( In1RecvData_arg, In2RecvData_arg); 
%
% 3. Review the added POSIX queue behavior functions. The main
% function uses these functions to open, close, and unlink the POSIX queue.
%
%  mqd_t openMQ(const char *name, int flags, int maxmsg, int msgsize)
%   {
%       struct mq_attr attr;
%       attr.mq_flags = 0;
%       attr.mq_maxmsg = maxmsg;
%       attr.mq_msgsize = msgsize;
%
%       mqd_t msgQueue = mq_open(name, flags, 0664, &attr);
%
%       if (msgQueue < 0) {
%           printf("mq_open failed\n");
%           exit(1);
%       }
%
%       return msgQueue;
%   }
%
%  void closeMQ(mqd_t msgQueue)
%   {
%       mq_close(msgQueue);
%   }
%   
%  void unlinkMQ(const char *name)
%   {
%       if (mq_unlink(name) < 0) {
%           printf("mq_unlink failed\n");
%           exit(1);
%       }
%   }
%
% 4. Review the main function. The implemented main 
% function uses the previously defined class and queue functions to receive
% messages and to manage the POSIX queue. The main function performs the following:
% 
% * Opens a POSIX message queue.
% * Receives messages.
% * Closes the POSIX queue.
%
%  int_T main(int_T argc, const char *argv[])
%   {
%       // Unused arguments
%       (void)(argc);
%       (void)(argv);
%
%       // Initialize model
%       rtObj.initialize();
%  
%       // Open POSIX message queues
%       mqd_t msgQueueAccel = openMQ("/PosixMQ_accel", O_CREAT | O_RDONLY, 2, 16);
%       mqd_t msgQueueGPS = openMQ("/PosixMQ_gpspos", O_CREAT | O_RDONLY, 2, 16);
%
%       In1RecvData_arg.SetMQ(msgQueueAccel);
%       In2RecvData_arg.SetMQ(msgQueueGPS);
%
%       // Receive messages
%       while (rtmGetErrorStatus(rtObj.getRTM()) == (NULL)) {
%           rt_OneStep();
%       }
%
%       // Matfile logging
%       rt_StopDataLogging(MATFILE, rtObj.getRTM()->rtwLogInfo);
%
%       // Disable rt_OneStep() here
%  
%       // Close POSIX message queues
%        closeMQ(msgQueueAccel);
%        closeMQ(msgQueueGPS);
%  
%        unlinkMQ("/PosixMQ_accel");
%        unlinkMQ("/PosixMQ_gpspos");
%  
%       return 0;
%   }
%

%% Execute Multisensor Positioning System
% To execute the multisensor positioning system, rebuild and run the
% executables for all models. To view results, 
% run the provided build script |rtwdemo_positioning_system_script|. An
% example of results is shown in this figure.
%
% 
% <<../result.png>>
% 
%%