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

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

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

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

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

Сгенерируйте код

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

2. Сгенерируйте код. На вкладке Код С++ нажмите Build.

Интеграция кода

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

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

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

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

// 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. Сгенерируйте код. На вкладке Код С++ нажмите Build.

Интеграция кода

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

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

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. Пример результатов показан на этом рисунке.