Модели датчиков

Первым компонентом в мультисенсорной системе позиционирования являются датчики. Система позиционирования использует акселерометр и GPS-датчик для оценки положения транспортного средства. Каждая модель датчика состоит из функционального блока, блока отправки и порта вывода. Каждая модель датчика посылает сообщения в модель оценки с использованием сообщений POSIX.

Подготовка моделей

Откройте модели датчиков rtwdemo_accel_send и rtwdemo_gps_send.

Для каждой модели выполните следующие действия.

Создать код

1. Для каждой модели датчика в диалоговом окне Параметры конфигурации (Configuration Parameters) задайте следующие параметры:

2. Создать код. На вкладке Код C++ нажмите кнопку Создать.

Интегрировать код

Чтобы интегрировать сгенерированный код C++ из каждой модели датчика с POSIX, вручную напишите реализацию класса отправки и сгенерированную основную программу (или создайте свою собственную). В этом примере представлена настраиваемая основная программа с реализованным классом отправки. Чтобы интегрировать код из каждой модели датчика, замените созданный основной файл настраиваемым основным файлом.

Просмотрите настроенный основной файл для каждого датчика:

1. Настраиваемый основной файл для каждого датчика реализован одинаково. В качестве примера откройте настраиваемый основной файл rtwdemo_accel_send_ert_main_customized.cpp.

2. Просмотрите реализованный класс отправки:

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

После реализации класса send создается экземпляр класса send (объект send). Затем объект send присоединяется к модели датчика, чтобы позволить датчику посылать сообщения в модель оценки.

// Create send object
static accelSendModelClassSendData_real_T Out1SendData_arg;

// Attach send object to sensor model to send messages
static accelSendModelClass rtObj( Out1SendData_arg);

3. Просмотрите добавленные функции поведения очереди POSIX. Основная функция использует эти функции для открытия и закрытия очереди POSIX.

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. Просмотрите основную функцию. Реализованная основная функция использует ранее определенные функции класса и очереди для отправки сообщений и управления очередью сообщений POSIX. Основная функция выполняет следующие функции:

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

Модель оценки

Вторым компонентом является модель оценки. Модель оценки принимает сообщения, которые имеют входные данные от моделей датчиков, и вычисляет расчетное положение транспортного средства.

Подготовка модели

Открытие модели rtwdemo_pos_estimate.

Создать код

1. В диалоговом окне «Параметры конфигурации» задайте следующие параметры:

2. Создать код. На вкладке Код C++ нажмите кнопку Создать.

Интегрировать код

Чтобы интегрировать сгенерированный код C++ из модели оценки с POSIX, вручную напишите реализацию класса приема и сгенерированную основную программу (или создайте свою собственную). Модель оценки принимает сообщения от моделей датчиков и вычисляет оценочное положение транспортного средства. В этих примерах представлена настраиваемая основная программа с реализованным классом приема. Чтобы интегрировать код из модели оценки, замените созданный основной файл настраиваемым основным файлом.

Просмотрите настроенный основной файл:

1. Открытие настраиваемого основного файла rtwdemo_pos_estimate_ert_main_customized.cpp.

2. Просмотрите внедренный класс получения:

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

После реализации класса приема создается экземпляр класса приема (объект приема) для каждой модели датчика. Затем объекты приема присоединяются к модели оценки, чтобы дать возможность модели принимать сообщения от каждого датчика.

// 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. Просмотрите добавленные функции поведения очереди POSIX. Основная функция использует эти функции для открытия, закрытия и разъединения очереди POSIX.

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. Просмотрите основную функцию. Реализованная основная функция использует ранее определенные функции класса и очереди для приема сообщений и управления очередью POSIX. Основная функция выполняет следующие функции:

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

Выполнение мультисенсорной системы позиционирования

Для выполнения мультисенсорной системы позиционирования перестроите и запустите исполняемые файлы для всех моделей. Чтобы просмотреть результаты, запустите предоставленный сценарий построения rtwdemo_positioning_system_script. Пример результатов показан на этом рисунке.